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Executive  Summary 


Unmanned  aerial  vehicle  and  smart  munition  systems  require  robust,  real-time 
path  generation  to  account  for  terrain  obstructions,  weather,  and  moving  threats 
such  as  radar,  jammers,  and  unfriendly  aircraft.  The  increasing  speed  and 
power  of  computational  resources  has  enabled  the  development  of  autonomous 
flight  control  systems  which  are  capable  of  dealing  with  the  complex  task  of 
path  planning  in  dynamic  and  uncertain  environments.  In  Phase  I  a  feasible, 
hierarchal  approach  for  real-time  motion  planning  of  autonomous  vehicles  was 
developed. 

Our  approach  decomposes  the  trajectory  generation  problem  into  three  tasks 
to  be  performed  by  the  UAV:  waypoint  path  planning  (WPP),  dynamic  trajec¬ 
tory  smoothing  (DTS),  and  adaptive  trajectory  tracking  (ATT).  The  WPP  plans 
paths  at  a  high  level  without  regard  for  the  dynamic  constraints  of  the  vehicle. 
This  affords  a  significant  reduction  in  the  search  space,  enabling  the  generation 
of  extremely  complicated  paths  that  account  for  pop-up  and  dynamically  chang¬ 
ing  threats.  The  essential  idea  of  the  DTS  is  to  give  the  trajectory  generator  a 
similar  mathematical  structure  as  the  physical  vehicle.  The  DTS  uses  a  simple, 
but  novel  algorithm  to  generate  smoothed  trajectories  in  real-time  without  per¬ 
forming  any  on-line  optimization.  The  trajectories  that  are  generated  by  the 
DTS  have  the  same  path  length  as  the  waypoint  path  generated  by  the  WPP 
and  also  minimize  the  deviation  from  the  waypoint  path.  The  third  step  uses 
an  adaptive  nonlinear  control  technique,  backstepping,  to  transform  the  trajec¬ 
tory  generated  by  the  DTS  to  a  feasible  trajectory  that  can  be  followed  by  an 
autopilot  with  appropriate  velocity,  altitude  and  heading  commands. 

The  benefits  of  the  approach  are: 

•  The  algorithms  are  computationally  efficient  and  can  handle  hundreds  of 
threats,  including  pop-up  threats. 

•  The  algorithms  do  not  require  any  on-line  optimization. 

•  The  approach  is  well  suited  for  applications  with  timing  constraints.  Plan¬ 
ning  can  take  place  at  the  waypoint  level,  where  it  is  trivial  to  calculate 
path  length,  and  therefore  the  estimated  time-of-arrival  (ETA).  The  DTS 
and  ATT  then  guarantee  that  the  ETA  remains  unchanged.  The  WPP  and 
DTS  have  been  used  successfully  in  coordinated  control  problems  where 
the  coordination  objective  is  to  sequence  the  ETA  of  different  vehicles.1 

•  The  trajectories  are  represented  in  a  compact  fashion,  in  both  space  and 
time.  In  particular,  this  allows  higher-level  task  planning  algorithms  to 
reason  about  the  feasibility,  or  desirability  of  different  trajectories. 


2 


Contents 

Executive  Summary  2 

Table  of  Contents  3 

1  Technical  Objectives  of  Phase  I  4 

2  Introduction  4 

3  Trajectory  Generation  Algorithm  Development  5 

3.1  Trajectory  Tracker .  6 

3.2  Trajectory  Smoother . -  8 

3.3  Waypoint  Path  Planning .  12 

3.3.1  Example .  15 

3.4  Phase  I  Accomplishments .  17 

3.4.1  Task  1:  Algorithm  Development  .  17 

3.4.2  Task  2:  Algorithm  Validation  and  Testing .  18 

3.4.3  Task  3:  Determine  Implementation  Requirements .  19 

3.5  Summary  of  Accomplishments  in  Phase  I .  22 

4  Plans  for  Phase  II  and  III  22 

5  Technology  Transitions  23 

6  Publications  23 

7  Personnel  Supported  24 

8  References  25 

9  Appendix  1:  Application  to  Coordinated  Control  29 

10  Appendix  2:  Algorithm  Codes  for  12  Degree  of  Freedom  Sim¬ 
ulation  30 

10.1  Matlab  Script  Files .  30 

10.2  Supporting  C  files  .  89 

10.3  Other  files . 135 


3 


1  Technical  Objectives  of  Phase  I 

The  overall  objectives  of  Phase  I  were  to  to  develop  concept  algorithms  for 
real-time  trajectory  generation  of  autonomous  nonlinear  flight  systems  and  de¬ 
termine  the  system  requirements  for  their  implementation.  Specifically: 

•  Develop  robust  algorithms  for  a  hierarchal  trajectory  generator  which  en¬ 
ables  a  UAV  to  plan  its  path  in  response  to  static  and  dynamic  threats. 

•  Determine  the  real-time  computational  requirement  for  such  algorithms, 
as  well  as  required  failure  management  and  redundancy. 

2  Introduction 

The  increasing  power  of  computational  resources  makes  possible  the  develop¬ 
ment  of  autonomous  flight  control  systems  which  are  capable  of  dealing  with 
the  complex  task  of  path  planning  in  dynamic  and  uncertain  environments. 
Unmanned  aerial  vehicles  (UAVs)  require  robust,  real-time  path  generation  to 
account  for  terrain  obstructions,  weather,  and  moving  threats  such  as  radar, 
jammers,  and  unfriendly  aircraft.  Such  path  planning  algorithms  and  route 
navigation  aids  are  needed  to  accomplish  envisioned  future  UAV  missions.2 

There  are  two  general  approaches  to  trajectory  generation:  interpolation 
of  a  trajectory  database  and  formulation  of  the  trajectory  as  the  solution  to 
an  optimal  control  problem.  Methods  which  query  and  interpolate  trajectory 
databases  fall  into  several  categories:  probabilistic  roadmaps,3  lazy  probabilis¬ 
tic  roadmaps,4  and  rapidly-exploring  trees.5’6  Probabilistic  roadmaps  are  path¬ 
planning  algorithms  which  consist  of  off-line  building  of  a  graph  of  uniformly 
spaced  randomly  selected  configurations  called  milestones.  A  recent  extension7 
of  the  probabilistic  roadmap  approach  uses  Lyapunov  function  scheduling  to  deal 
with  system  dynamics  in  an  environment  with  moving  obstacles.  For  aerospace 
systems  with  complex  high-dimensional  dynamics,  this  motion  planning  ap¬ 
proach  is  based  on  a  quantization  of  system  dynamics  into  a  library  of  feasible 
trajectory  primitives.8 

Trajectory  generation  via  numerical  solution  of  optimal  control  problems9-12 
is  computationally  intensive  and  requires  recently  developed  techniques  from  ge¬ 
ometric  nonlinear  control  theory  for  feasible  implementation.  These  techniques 
are  based  on  finding  a  differentially  flat  output  for  the  system.13-15  From  such 
an  output  and  its  derivatives,  the  complete  differential  behavior  of  the  system 
can  by  reconstructed.  In  Ref.  16, 17,  a  flat  output  is  used  to  find  a  lower  di¬ 
mensional  space  in  which  trajectory  curves  are  generated  using  B-splines  and 
sequential  quadratic  programming.  In  a  similar  vein,  Refs.  18, 19  transform 
the  nonlinear  optimization  problem  to  a  linear  one  using  feedback  linearization, 
which  requires  finding  a  flat  output,  making  it  possible  to  convert  constrained 
dynamic  optimization  problems  into  unconstrained  ones.  In  Ref.  20,  the  differ¬ 
ential  flatness  property  was  used  to  develop  an  iterative  approach  to  finding  a 
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feasible  solution  which  satisfies  terminal  path  constraints  using  an  H estima¬ 
tor. 

The  path  planner  developed  in  Phase  I  uses  a  modified  Voronoi  diagram21  to 
generate  possible  paths.  The  Voronoi  diagram  is  then  searched  via  Eppstein’s 
fc-best  paths  algorithm.22  Similar  path  planners  have  been  previously  reported 
in  Refs.  23-25.  The  basic  idea  is  to  plan  a  polygonal  path  through  a  set  of 
threats  using  a  Voronoi  algorithm  in  connection  with  an  A*  or  Dijkstra  algo¬ 
rithm.  The  polygonal  paths  are  then  made  flyable  by  a  trajectory  smoother  that 
dynamically  smooths  through  the  corners  of  the  paths  such  that  the  curvature 
of  the  smoothed  path  is  flyable  by  the  UAV.  The  described  algorithm  works  well 
when  the  minimum  turning  radius  is  small  compared  to  the  path  links. 


3  Trajectory  Generation  Algorithm  Development 

The  overall  architecture  is  built  around  a  systematic  division  of  the  problem  into 
five  distinct  hierarchical  layers:  path  planning,  trajectory  smoothing,  trajectory 
tracking,  autopilot,  and  the  UAV.  In  this  paper,  paths  refer  to  a  series  of  way- 
points  which  are  not  time-stamped,  while  trajectories  will  refer  to  time-stamped 
curves  which  specify  the  desired  inertial  location  of  the  UAV  at  a  specified  time. 
Figure  1  shows  a  schematic  of  the  architecture.  At  the  top  level  is  a  path  plan- 
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Figure  1:  Proposed  system  architecture. 

ner  (PP).  It  is  assumed  that  the  PP  knows  the  location  of  the  UAV,  the  target, 
and  the  location  of  a  set  of  threats.  The  PP  generates  a  path 

V  =  {v,  {wi,w2,...,w;v}}, 

where  v  €  [vmin,vmax]  is  a  feasible  velocity  and  {w*}  is  a  series  of  waypoints 
which  define  straight-line  segments  along  which  the  UAV  attempts  to  fly.  Note 
that  at  the  PP  level,  the  path  is  compactly  represented  by  V.  Higher  level 
decision  making  algorithms  reason  and  make  decisions  according  to  the  data 
represented  in  V . 

The  Trajectory  Smoother  (TS)  transforms,  in  real-time,  the  waypoint  trajec¬ 
tory  into  a  time  parameterized  curve  which  defines  the  desired  inertial  position 
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of  the  UAV  at  each  time  instant.  The  output  of  the  TS  is  the  desired  trajectory 
z d(t)  =  (zj|(t),  zd(t))T  at  time  t. 

The  Trajectory  Tracker  (TT)  transforms  z d(t)  into  desired  velocity  command 
Vdy  altitude  command  hd ,  and  heading  command  The  autopilot  receives 
these  commands  and  controls  the  elevator,  8e ,  and  aileron,  8a,  deflections  and 
the  throttle  setting  5t. 

Recognizing  that  it  will  be  useful  for  human  operators  to  interact  with  the 
UAV  at  different  autonomy  levels,  careful  attention  has  been  given  to  the  human 
interface.  As  shown  in  Figure  1,  the  human  can  interact  with  the  UAV  at  the 
stick-and-throttle  level,  the  autopilot  command  level,  the  time-parameterized 
trajectory  level,  or  at  the  waypoint  path  planning  level. 

3.1  Trajectory  Tracker 

This  section  provides  a  description  of  the  trajectory  tracker.  A  complete  de¬ 
scription  is  contained  in  Refs.  26,27.  We  will  assume  that  the  UAV/autopilot 
combination  is  adequately  modeled  by  the  kinematic  equations 

x  =  vc  cos(,0) 

y  =  vc  sin(V0  (1) 

^  =  uc, 

where  {x,y)  is  the  inertial  position  of  the  UAV,  is  As  heading,  vc  is  the 
commanded  linear  speed,  and  ljc  is  the  commanded  heading  rate.  The  dynamics 
of  the  UAV  impose  input  constraints  of  the  form 

0  <  Vmin  ^  Umax 

—Umax  <UC  <  CO  max  •  (2) 

As  we  will  describe  in  the  next  section,  the  trajectory  generator  produces  a 
reference  trajectory  that  satisfies 


xr  =  vr  cos(Vv) 

yr  =  vr  sin(Vv)  (3) 

Ipr  =  Ur 

under  the  constraints  that  vr  and  u>r  are  piecewise  continuous  and  satisfy  the 
constraints 


Vmin  H"  <  vmax  €v  (4) 

Umax  4”  <Ur  <  Umax  i 

where  ev  and  €w  are  positive  control  parameters. 

The  trajectory  tracking  problem  is  complicated  by  the  nonholonomic  nature 
of  Equations  (1)  and  the  input  constraint  on  the  commanded  speed  vc. 
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The  first  step  in  the  design  of  the  trajectory  tracker  is  to  transform  the 
tracking  errors  to  the  UAV  body  frame  as  follows: 


XG 

cos(V0  sin(V0  0 

Xr  —  X 

Ve 

= 

—  sin  (ip)  cos  (VO  0 

yr-y 

.  ^ e  . 

o  01 

m  1pr- Ip  m 

Accordingly,  the  tracking  error  model  can  be  represented  as 


Xe  =  UCye  -VC  +  Vr  COs(V'e) 
ye  -  -u)cxe  4-  Vr  sin(^e) 

Ip  =  UJr  —  LJC . 

Following  Ref.  28,  Eq.  (6)  can  be  simplified  to 


x0  =  Uq 

x\  =  (ur  -  u0)x2  +  vr  sin(x0) 

X2  =  -(u>r  -  Uq)xi  +  U\, 


where 


and 


"  Xq 

V*e 

X\ 

= 

ye 

-  X 2  - 

m  -Xe 

W0 

cjr  —  u;c 

i>c  —  tv  cos(xo) 

The  input  constraints  under  the  transformation  become 


(6) 


(7) 

(8) 


€(jj  ^  Uq  ^  €.fjj 

U min  Ur  COs(xo)  ^  U\  ^  'Umax  ^ r  COs(xo).  (0) 

Obviously,  Eqs.  (5)  and  (8)  are  invertible  transformations,  which  means  ( xq ,  aq,  X2) 
(0, 0, 0)  is  equivalent  to  (xe,  ye^e)  =  (0, 0, 0)  and  (xr ,  yr ,  Vv)  =  (z,  y ,  VO*  There¬ 
fore,  the  original  tracking  control  objective  is  converted  to  a  stabilization  ob¬ 
jective,  that  is,  our  goal  is  to  find  feasible  control  inputs  uq  and  u\  to  stabilize 
xq  ^  x~^ ,  and  X2* 

Note  from  Eq.  (7)  that  when  both  xq  and  X2  go  to  zero,  that  x\  becomes 
uncontrollable.  To  avoid  this  situation  we  introduce  another  change  of  variables. 

Let  xi 

xq  —  mx  0  H - ,  (10) 

7Ti 

where  m  >  0  and  iti  =  ^ x\  +  x\  4  1.  Accordingly,  xq  —  —  ~±.  Obviously, 

(x0,xi,X2)  =  (0,0,0)  is  equivalent  to  (x0,xi,X2)  =  (0,0,0).  Therefore  it  is 


7 


sufficient  to  find  control  inputs  uq  and  u\  to  stabilize  xo ,  xi,  and  x2 .  With  the 
same  input  constraints  (9),  Eq.  (7)  can  be  rewritten  as 


^  x2  v  .  ^2  , 

xo  =  (m - )u0  -f - u V 

7Ti  7Ti 


l+Xo  .  /®0  *1  \  xlx2 

H - sin - - 3-^1 

7Tj  \  m  m7Ti  /  7TJ 

xi  =  (wr  —  t/o)^2  4-  t>r  sin(— - — ) 

v  '  m  mix  1 

X2  =  -~(cjr  —  U0)x  1  +  1*1. 


(11) 


In  Refs.  26,27  we  have  shown  that  if 


|f?ozo|  <  eu> 

— sign(x0)ew,  |r?0x0|  >  eu 

tL> 

—771X2  <  V 

-mx  2, 

V  <  —7/1X2  <  v  , 

V, 

—7/1 X2  >  V 

(12) 

(13) 


where  v  =  vmin  -  vr  cos  (ff  -  and  v  =  vmax  -  vr  cos  (fj  -  ^),  and 

rjo  and  7/1  are  sufficiently  large  (made  precise  in  Refs.  26,27),  then  the  tracking 
error  goes  to  zero  asymptotically. 

Note  the  computational  simplicity  of  Equations  (12)-(13).  We  have  found 
that  the  tracker  can  be  efficiently  implemented  on  the  autopilot  hardware  dis¬ 
cussed  in  Section  3.4.3. 

Figure  2  shows  a  reference  trajectory  of  the  UAV  in  green  and  the  actual 
UAV  trajectory  in  blue.  Figure  3  plots  the  tracking  errors  verses  time  and 
demonstrates  the  asymptotically  stable  characteristics  of  the  trajectory  tracker. 


3.2  Trajectory  Smoother 

Given  a  path  V,  the  objective  of  the  Dynamic  Trajectory  Smoother  (DTS)  is  to 
generate  time-parameterized  trajectories  that  are  feasible  within  the  dynamic 
constraints  of  the  UAV.  The  essential  idea  is  to  use  a  nonlinear  filter  that  has 
a  mathematical  structure  similar  to  the  dynamic  structure  of  the  vehicle  to 
generate  trajectories  that  smooth  through  the  waypoints  in  real-time,  while  the 
vehicle  traverses  the  trajectory. 

The  Trajectory  Smoother  (TS)  translates  a  straight-line  path  into  a  feasible 
trajectory  for  a  UAV  with  velocity  and  heading  rate  constraints.  Our  particular 
implementation  of  trajectory  smoothing  also  has  some  nice  theoretical  properties 
including  time-optimal  waypoint  following.29’30 

We  start  by  assuming  that  an  auto-piloted  UAV  is  modeled  by  the  kinematics 
equations  given  in  Eq.  (1),  with  the  associated  constraints  given  in  Eq.  (2). 
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Figure  2:  The  simulation  scenario:  waypoint  path  (green),  smoothed  reference 
trajectory  (red),  and  actual  trajectory  (blue). 

The  fundamental  idea  behind  feasible,  time-extremal  trajectory  generation 
is  to  impose  on  TS  a  mathematical  structure  similar  to  the  kinematics  of  the 
UAV.  The  structure  of  the  TS  is  given  in  Eq.  (3)  with  constraints  given  by 
Eq.  (4).  To  simplify  notation,  let  c  =  a>max  “ 

With  the  velocity  fixed  at  v ,  the  minimum  turn  radius  is  defined  as  R  —  v/c. 
The  idea  of  a  minimum  turn  radius  allows  us  to  visualize  the  area  of  space  that 
the  UAV  can  reach  in  the  next  instant  of  time,  i.e.  the  local  reachability  region. 

With  this  in  mind,  it  seems  natural  that  for  a  trajectory  to  be  time-optimal, 
it  will  be  a  sequence  of  straight-line  path  segments  combined  with  arcs  along 
the  minimum  radius  circles  (i.e.  along  the  edges  of  the  local  reachability  re¬ 
gion).  In  fact,  Anderson  proved  in  Ref.  31  that  this  is  the  case.  By  postulating 
that  cvr  follows  a  bang-bang  control  strategy  during  transitions  from  one  path 
segment  to  the  next,  he  showed  that  a  k- trajectory  is  time-extremal,  where  a 
/c-trajectory  is  defined  as  follows.  As  shown  in  Figure  5,  a  K-trajectory  is  defined 
as  the  trajectory  that  is  constructed  by  following  the  line  segment  w^iw*  until 
intersecting  Ci ,  which  is  followed  until  Cp(*)  is  intersected,  which  is  followed  until 
intersecting  Ci+\  which  is  followed  until  the  line  segment  wJwTJI  is  intersected. 

Note  that  different  values  of  k  can  be  selected  to  satisfy  different  require¬ 
ments.  For  example,  k  can  be  chosen  to  guarantee  that  the  UAV  explicitly 
passes  over  each  waypoint,  or  k  can  be  found  by  a  simple  bi-section  search  to 
make  the  trajectory  have  the  same  length  as  the  original  straight-line  path,31 
thus  facilitating  timing-critical  trajectory  generation  problems. 

The  TS  implements  ^-trajectories  to  follow  waypoint  paths.  In  evaluating 
the  real-time  nature  of  the  TS,  we  chose  to  require  trajectories  to  have  equal 
path  length  as  the  initial  straight  line  paths.  The  computational  complexity  to 


Figure  3:  The  trajectory  tracking  errors  expressed  in  the  inertial  frame. 


find  u ;r  is  dominated  by  finding  circle-line  and  circle-circle  intersections.  Since 
the  TS  also  propagates  the  state  of  the  system  in  response  to  the  input  ujr, 
Eq.  (3)  must  be  solved  in  real-time.  This  is  done  via  a  forth-order  Runge-Kutta 
algorithm.32  In  this  manner,  the  output  of  the  TS  corresponds  in  time  to  the 
evolution  of  the  UAV  dynamics  and  ensures  a  time-optimal  trajectory. 

Figure  6  shows  the  essential  idea  of  the  DTS  algorithm  which  was  recently 
developed  in.31  Let  p  be  a  point  on  the  bisector  of  the  angle  defined  by  the 
path  segments  £{  =  (wi_i,Wi)  and  1  =  (w^w^+i),  and  let  Cp  denote  the 
circle  of  radius  R  centered  at  p.  The  DTS  tracks  ii  until  CL  intersects  Cp  at 
point  zi,  upon  which  u  is  set  to  -fc.  The  DTS  follows  the  circle  defined  by 
Cl  until  it  reaches  Z2>  upon  which  it  sets  u  =  —c.  The  circle  defined  by  Cp  is 
followed  until  the  DTS  reaches  13,  upon  which  u  =  +c  until  24  where  u  =  0. 
Using  Pont ry gin’s  minimum  principle,  it  was  shown  in31  that  the  trajectories 
generated  by  this  algorithm  are  time-extremal  trajectories.  A  novel  aspect  of 
this  algorithm  is  that  the  point  p  can  be  adjusted  to  find  the  time-extremal 
trajectory  whose  path  length  is  exactly  equal  to  the  corresponding  path.31  Let 
k  £  [0, 1]  be  a  dimensionless  parameterization  of  the  distance  from  the  waypoint 
to  an  inscribed  circle  C  whose  center  lies  along  the  bisector  of  the  angle.  The 
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Local  reachability  region 
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Figure  4:  Local  reachability  region  of  the  TS. 
the  value  of  k  corresponding  to  an  equal  length  trajectory  is  given  by 


where  (3  €  [0,  n)  is  the  angle  between  the  path  segments  £{  and  £i+\  as  shown  in 
Figure  6.  The  function  A  can  be  shown  to  be  monotonic,  therefore  its  unique 
roots  can  be  found  quickly  via  a  bisection  search  algorithm. 

There  are  several  advantages  to  the  DTS  approach.  First,  it  is  nicely 
matched  to  the  WPP  algorithms  described  in  the  previous  section.  Second,  the 
approach  has  low  computational  overhead.  In  fact,  trajectories  are  generated 
in  real-time,  as  the  vehicle  moves.  Third,  it  can  be  shown  using  optimal  control 
techniques  that  the  approach  minimizes  the  time  that  the  vehicle  deviates  from 
the  Voronoi  path.  Finally,  it  can  be  adapted  to  allow  low-level  deconfliction,  and 
to  allow  threat  avoidance  for  dynamic  threats.  The  algorithm  described  above 
can  be  modified  using  behavior-based  approaches,33  to  allow  local  adjustments 
to  the  path  in  response  to  dynamically  moving  threats. 

Hardware  implementation  of  the  TS  has  shown  the  real-time  capability  of 
this  approach.  On  a  1.8  GHz  Intel  Pentium  4  processor,  one  iteration  of  the  TS 
took  on  average  36  ^-seconds.  At  this  speed,  the  TS  could  run  at  25  KHz  -  well 
above  the  dynamic  range  of  typical  UAVs.  Moving  toward  embedded  systems, 
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Figure  5:  A  dynamically  feasible  K-trajectory. 


we  found  that  one  iteration  of  the  TS  required  a  maximum  of  47  milli-seconds  on 
a  Rabbit  Microprocessor  running  at  29  MHz.  The  low  computational  demand 
allows  the  TS  to  be  run  in  real-time  at  approximately  20  Hz  on  an  embedded 
system  on-board  the  UAVs. 

3.3  Waypoint  Path  Planning 

For  many  anticipated  military  and  civil  applications,  the  capability  for  a  UAV 
to  plan  its  route  as  it  flies  is  important.  Reconnaissance,  exploration,  and  search 
and  rescue  missions  all  require  the  ability  to  respond  to  sensed  information  and 
to  navigate  on  the  fly. 

In  the  flight  control  architecture  shown  in  Figure  1,  the  coarsest  level  of 
route  planning  is  carried  out  by  the  path  planner  (PP).  Our  waypoint  planning 
technique  centers  around  the  construction  and  search  of  a  Voronoi  graph.24  The 
Voronoi  graph  provides  a  method  for  creating  waypoint  paths  through  threats 
or  obstacles  in  the  airspace.  A  prime  advantage  of  the  Voronoi  graph  is  the 
computational  speed  with  which  the  graph  can  be  created  and  searched. 

In  Phase  I,  we  have  modelled  threats  and  obstacles  in  two  different  ways: 
as  points  to  be  avoided  and  as  polygonal  regions  that  cannot  be  entered.  With 
threats  specified  as  points,  construction  of  the  Voronoi  graph  is  straightforward 
using  existing  algorithms.  For  an  area  with  n  point  threats,  the  Voronoi  graph 
will  consist  of  n  convex  cells,  each  containing  one  threat.  Every  location  within 
a  cell  is  closer  to  its  associated  threat  than  to  any  other.  By  using  threat 
locations  to  construct  the  graph,  the  resulting  graph  edges  form  a  set  of  lines 
that  are  equidistant  from  the  closest  threats.  In  this  way,  the  edges  of  the  graph 
maximize  the  distance  from  the  closest  threats.  Figure  7  shows  an  example  of 
a  Voronoi  graph  constructed  from  point  threats. 


Figure  6:  Graphical  depiction  of  the  essential  idea  behind  the  DTS. 


Construction  of  a  Voronoi  graph  for  obstacles  modelled  as  polygons  is  an 
extension  of  the  point-threat  method.  In  this  case,  the  graph  is  constructed 
using  the  vertices  of  the  polygons  that  form  the  periphery  of  the  obstacles.  This 
initial  graph  will  have  numerous  edges  that  cross  through  the  obstacles.  To 
eliminate  these  infeasible  edges,  a  pruning  operation  is  performed.  Using  a  line 
intersection  algorithm,  those  edges  that  intersect  the  edges  of  the  polygon  are 
detected  and  removed  from  the  graph.  Figure  8  shows  the  initial  polygon  based 
graph  and  the  final  graph  after  pruning. 

Finding  good  paths  through  the  Voronoi  graph  requires  the  definition  of  a 
cost  metric  associated  with  travelling  along  each  edge.  In  our  work,  two  metrics 
have  been  employed:  path  length  and  threat  exposure.  A  weighted  sum  of 
these  two  costs  provides  a  means  for  finding  safe,  but  reasonably  short  paths. 
Although  the  highest  priority  is  usually  threat  avoidance,  the  length  of  the  path 
must  be  considered  to  prevent  safe,  but  meandering  paths  from  being  chosen. 

Once  a  metric  is  defined,  the  graph  is  searched  using  an  Eppstein  search.22 
This  is  an  computationally  efficient  search  with  the  ability  to  return  k  best 
paths  through  the  graph.  Once  k  best  paths  are  known,  a  coordination  agent  can 
choose  which  path  to  choose  for  each  vehicle  in  the  team  (to  ensure  simultaneous 
arrival,  for  example).  The  points  of  this  chosen  path  are  passed  on  the  the 
trajectory  generator  which  smooths  through  the  path,  taking  into  account  the 
dynamics  and  constraints  of  the  vehicle. 

Each  edge  of  the  Voronoi  diagram  is  assigned  two  costs:  a  threat  cost  and 
a  length  cost.  Threat  costs  are  based  on  a  UAV’s  exposure  to  enemy  radar. 
Assuming  that  the  UAV  radar  signature  is  uniform  in  all  directions  and  is  pro¬ 
portional  to  1/d4  (where  d  is  the  distance  from  the  UAV  to  the  threat),  the 
threat  cost  for  travelling  along  an  edge  is  inversely  proportional  to  the  distance 
(to  the  threat)  to  the  fourth  power.  An  exact  threat  cost  calculation  would  in¬ 
volve  the  integration  of  the  cost  along  each  edge.  An  acceptable  approximation 
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Figure  7:  Voronoi  graph  with  point  threats 


Figure  8:  Voronoi  graph  before  and  after  pruning  with  polygon  threats 


is  to  calculate  the  threat  cost  at  several  locations  along  an  edge  and  take  the 
length  of  the  edge  into  account.  In  this  work,  the  threat  cost  was  calculated  at 
three  points  along  each  edge:  Li/6,  Li/2,  and  5L*/6,  where  Li  is  the  length  of 
edge  i .  The  threat  cost  associated  with  the  \th  edge  is  given  by  the  expression 


T  r  ^  f  1  ,  1  1  \ 

Jthreat,i  ~  Li  ^  I  ,4  +  ,4  +14  I  » 

j=l  \ai/6 ,ij  ai/2 ,i,j  a5/6,i,i/ 


(14) 


where  N  is  the  total  number  of  threats  and  di/2tij  is  the  distance  from  the  1/2 
point  on  the  1th  edge  to  the  ]th  threat.  To  include  the  path  length  as  part  of 
the  cost,  the  length  cost  associated  with  each  edge  is 


Jlength,i  —  Li.  (15) 

The  total  cost  for  travelling  along  an  edge  comes  from  a  weighted  sum  of  the 
threat  and  length  costs: 


Ji  —  kJlength,i  “b  (1  ^)*/t/ireot,i* 


(16) 


The  choice  of  k  between  0  and  1  gives  the  designer  flexibility  to  place  weight 
on  exposure  to  threats  or  fuel  expenditure  depending  on  the  particular  mission 
scenario.  With  the  cost  determined  for  each  of  the  Voronoi  edges,  the  Voronoi 
diagram  is  searched  to  find  the  lowest-cost  path  using  Eppstein’s  algorithm.22 
Figure  2  shows  the  five  best  paths  returned  from  a  search  of  the  Voronoi  diagram. 

Taken  in  different  combinations,  the  edges  of  the  Voronoi  diagram  provide 
a  rich  set  of  paths  from  the  starting  point  to  the  target.  A  great  advantage 
of  the  Voronoi  diagram  approach  is  that  it  reduces  the  path-planning  problem 
from  an  infinite-dimensional  search,  to  a  finite-dimensional  graph  search.  This 
important  abstraction  makes  the  path-planning  problem  feasible  in  real-time. 

Voronoi  diagram  generation  and  search  can  be  performed  in  a  computation¬ 
ally  efficient  manner,  generating  a  path  in  about  10  millisecond  for  battlefields 
with  approximately  100  threats. 

The  problem,  of  course,  is  that  straight-line  paths  are  not  feasible  within 
the  dynamic  constraints  of  the  UAV.  The  objective  of  the  Dynamic  Trajectory 
Smoother  is  to  transform  V  into  a  dynamically  feasible  trajectory. 

There  are  numerous  applications  where  timing  is  critical.  For  example,  it 
may  be  desirable  that  two  or  more  UAVs  strike  a  target  simultaneously  to 
maximize  the  element  of  surprise.  The  waypoint  paths,  together  with  a  velocity 
v  e  [%in5Vmax]  specify  the  time  of  completion.  However,  in  smoothing  the 
trajectory,  if  the  path  length  is  changed,  the  timing  will  be  altered.  Therefore 
if  timing  is  critical,  it  is  necessary  to  smooth  the  trajectories  in  such  a  way  that 
path  length  is  not  altered. 

3.3.1  Example 

In  this  section  we  will  illustrate  the  essential  ideas  via  a  Simulink  simulation 
of  the  WPP/DTS  combination.  Figure  9  shows  a  battlefield  scenario  where  a 
single  UAV  is  assigned  to  visit  several  target  locations.  In  the  battlefield  there 
are  36  known  threat  locations  with  several  unknown  threat  locations  that  are 
detected  by  the  UAV  when  it  flies  in  the  vicinity  of  the  threat.  We  assume 
that  the  UAV  is  initially  at  position  (-25  km,  5  km)  and  that  the  first  target 
location,  represented  by  a  small  box,  is  at  (20  km,  5  km).  The  dotted  line 
in  Figure  9(a)  is  the  initial  waypoint  path  planned  by  the  WPP.  The  solid  line 
shows  the  trajectory  produced  by  the  DTS  from  the  initial  time  until  the  instant 
immediately  before  the  detection  of  the  first  pop-up  threat.  In  Figure  9(b)  a 
new  threat  at  location  (-15  km,  -2  km)  has  been  detected  by  the  UAV.  The 
new  waypoint  path  is  shown  in  the  dotted  line.  Figure  9(c)  shows  the  state  of 
the  WPP/DTS  immediately  after  a  second  pop-up  threat  has  been  detected  at 
position  (-8  km, 5  km).  Finally  Figure  9(d)  shows  the  state  of  the  WPP/DTS  at 
the  instant  of  time  when  the  first  target  location  has  been  reached.  The  WPP 
has  planned  a  waypoint  path,  represented  by  the  dotted  line,  to  the  second 
target  which  is  represented  by  a  square. 
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(a)  First  pop-up  threat  is  about  to  be  de¬ 
tected. 


r 


(b)  The  first  pop-up  threat  is  detected. 


(c)  The  second  pop-up  threat  is  detected.  (d)  The  first  target  is  reached. 


Figure  9:  Pop-up  Scenario 


3.4  Phase  I  Accomplishments 

Real-time  path  planning  algorithms  were  developed  which  generate  trajectories 
are  flyable  by  the  UAV,  computed  in  real-time,  and  can  respond  in  real-time  to 
pop-up  threats  and  dynamically  moving  The  trajectories  can  be  represented  in 
a  compact  fashion,  in  both  space  and  time.  In  particular,  this  allows  higher- 
level  task  planning  algorithms  to  reason  about  the  feasibility,  or  desirability  of 
different  trajectories. 

3.4.1  Task  1:  Algorithm  Development 

1.  A  way  point  path  planner  (WPP)  was  designed  to  accommodate  dynami¬ 
cally  moving  threats.  The  waypoints  move  dynamically  to  guide  the  UAV 
to  avoid  moving  threats.  To  overcome  some  of  the  limitations  of  the 
Voronoi  approach  to  waypoint  path  planning,  the  path  planner  was  mod¬ 
ified  to  for  account  polygonal  no-go  regions  and  finite  threat  radius.  The 
modified  WPP  creates  a  tree  of  path  segments  by  connecting  the  current 
position  of  the  UAV  with  the  desired  target  via  a  straight  line.  If  the 
line  intersects  with  a  threat,  the  original  line  is  removed  from  the  tree 
and  four  new  lines  are  added  that  avoid  the  threat.  These  new  lines  are 
then  searched  recursively  to  ensure  that  they  don’t  intersect  with  threats. 
When  a  specified  number  of  paths  exist  from  the  start  to  the  target  (i.e. 
the  graph  is  ’full  enough’)  then  the  tree  is  searched  for  the  shortest  path. 

In  a  majority  of  cases,  this  approach  is  much  faster  than  our  initial  Voronoi 
approach  and  produces  a  short,  more  intuitive  path.  Since  the  time  to 
compute  is  small,  it  can  be  used  in  dynamic  threat  areas.  The  planning 
would  simply  be  done  at  every  time  step  with  the  threats  extended  in  their 
respective  directions  of  motion.  In  this  way,  the  algorithm  could  plan  with 
some  sort  of  prediction. 

There  are  cases  when  this  approach  might  fails  to  complete  in  a  timely 
manner.  When  threats  are  very  dense  or  the  starting  or  target  positions 
are  enclosed  in  concave  threat  areas,  then  the  algorithm  may  create  too 
many  paths  to  be  searched  efficiently.  These  issues  can  be  addressed  by 
grouping  overlapping  threats  into  convex  hulls  and  identifing/removing 
areas  where  many  edges  exist. 

The  path  planner  has  been  extended  to  handle  polygon  threat  regions 
to  account  for  no-fly  zones  and  known  threat  areas  of  arbitrary  shape. 
Improved  error  checking  enables  the  planner  to  plan  paths  in  previously 
problematic  situations  (i.e.  not  enough  threats  for  full  Voronoi  graph). 
Recent  Monte-Carlo  simulations  indicate  this  is  feasible  in  real-time:  for 
30  8-sided  polygons,  the  average  run  time  is  less  than  40  ms.  Over  200 
threats  could  be  included  within  a  second  of  computation. 

A  trajectory  tracker  that  explicitly  accounts  for  turning  rate  and  velocity 
constraints  was  integrated  with  a  12  degree-of-freedom  (DOF)  aircraft 
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model.  Algorithm  performance  and  behavior  with  this  higher-order  UAV 
model.  The  tracker  was  initially  developed  assuming  a  6  DOF  model. 

The  required  bandwidth  separation  between  layers  in  our  hierarchy  was 
carefully  investigated.  To  address  this,  a  simulation  was  developed  in 
which  the  points  passed  to  the  trajectory  smoother  were  changed  randomly 
at  every  time  step.  Results  show  that  no  matter  the  input  to  the  trajectory 
smoother,  sensible  outputs  are  generated.  In  fact,  the  output  will  always 
satisfy  the  heading  rate  and  velocity  constraints  of  the  UAV.  When  the 
random  point  generation  is  switched  to  deliberate  point  planning,  the 
smoother  quickly  tracks  the  path. 

The  algorithms  were  also  tested  via  simulation  using  a  cooperative  multi¬ 
vehicle  scenario  where  a  team  of  unmanned  air  vehicles  (UAV s)  is  given 
the  task  of  searching  a  region  with  unknown  opportunities  and  hazards. 
Opportunities  and  hazards  were  identified  in-flight,  within  the  immedi¬ 
ate  sensor  radius  of  the  UAVs,  and  are  therefore  constantly  “popping- 
up.”  The  team  objective  was  to  “visit”  as  many  opportunities  as  possible, 
while  “avoiding”  as  many  hazards  as  possible.  Since  opportunities  and 
hazards  were  constantly  appearing  on  the  sensor  horizon,  the  waypoint 
paths  that  achieve  the  stated  objective  will  also  be  changing  in  response 
to  new  information.  It  was  assumed  that  the  problem  is  essentially  two 
dimensional  so  collision  avoidance  must  be  accounted  for  explicitly.  A 
cooperative  waypoint  path  planner  for  this  scenario.  The  trajectory  gen¬ 
erator  and  trajectory  tracker  is  currently  being  integrated  with  the  path 
planner.  This  problem  was  used  to  study  the  bandwidth  separation  issue 
between  the  waypoint  path  planner  and  the  trajectory  generator. 

2.  An  adaptive  control  law  for  the  trajectory  tracker  was  synthesized  using 
Lyapunov  based-nonlinear  control  techniques.  The  additional  complexity 
added  to  the  control  law  is  minimal.  The  average  run-time  per  sample 
period  was  14.3  milliseconds  for  a  400  MHz  Pentium  II  and  105  millisec¬ 
onds  for  a  50  MHz  Rabbit  microcontroller.  The  initial  algorithm  was 
modified  to  explicitly  account  for  velocity  and  heading  rate  constraints. 
A  description  of  the  algorithm  will  be  submitted  for  possible  presentation 
at  the  2003  Conference  on  Decision  and  Control.  The  trajectory  tracker 
was  tested  in  simulation  has  also  using  mobile  robots  and  a  using  full  12 
degree-of-freedom  (DOF)  aircraft  model. 

3.4.2  Task  2:  Algorithm  Validation  and  Testing 

A  12  DOF  aircraft  dynamic  model  was  used  to  test  the  WPP  and  ATT  al¬ 
gorithms  via  simulation.  In  porting  the  algorithms  from  Mat lab/S imulink  to 
C/C++  we  remained  cognizant  of  the  fact  that  eventual  deployment  of  the  code 
will  require  extensive  verification  and  validation.  Therefore  the  WPP,  DTS,  and 
ATT  code  were  designed  with  verification  and  validation  in  mind.  To  facilitate 
testing,  all  UAV  and  algorithm  parameters  were  defined  in  configuration  text 
files.  This  allowed  a  process  to  randomly  generate  different  scenarios  without 
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recompiling  the  code.  It  was  in  this  way  that  Monte-Carlo  simulations  were  per¬ 
formed.  Algorithms  performance  issues  analyzed  include  average  speed,  speed 
vs  threat  density,  the  existence  of  feasible  path  vs.  threat  density. 

The  results  for  the  WPP  algorithm  on  a  400MHz  Pentium  II  (averaged  over 
200  random  threat  locations,  for  each  threat  density)  are  shown  below  in  figure 
2.  In  this  figure,  100%  density  corresponds  to  30  threats.  It  can  be  seen, 
that  even  at  this  level,  the  run-time  is  low  enough  that  we  can  re-plan  paths 
dynamically  as  threats  pop-up  and  move  dynamically. 


Figure  10:  Compute  time  for  Voronoi  waypoint  path  planner  verses  threat  den¬ 
sity. 

The  upper  bound  of  one  iteration  of  the  DTS  algorithm  on  a  1.8  GHz  Pen¬ 
tium  4  processor  is  63  micro-seconds.  This  corresponds  to  approximately  3 
milli-seconds  on  a  50  MHz  processor.  Run  time  for  DTS  is  dependent  only 
on  the  part  of  the  turn  that  the  UAV  is  executing.  In  most  cases  the  time  is 
negligible. 

3.4.3  Task  3:  Determine  Implementation  Requirements 

As  preparation  for  Phase  II,  system  requirements  for  actual  implementation  in 
future  combat  were  investigated.  As  part  of  this  effort,  the  WPP  and  DTS 
Matlab/Simulink  code  were  ported  into  real-time  C  code.  The  C/C++  im¬ 
plementation  matches  the  Matlab  implementation  with  functionality  preserved. 


The  advantages  of  having  the  DTS  implemented  in  C/C++  code  are  2  fold: 
speed  and  size.  On  a  Pentium  4  processor,  one  iteration  of  the  algorithm  has  an 
upper  bound  of  63  micro-seconds.  This  roughly  corresponds  to  3  milliseconds  on 
a  50  MHz  Rabbit  processor  -  the  micro-processor  onboard  the  prototype  UAV. 
The  code  executable  size  is  82K,  which  easily  fits  the  1M  of  memory  onboard 
the  UAV.  The  DTS  handles  cases  when  the  length  of  straight-line  path  segments 
is  large  compared  to  the  turning  radius  of  the  UAV.  Originally,  the  algorithm 
was  sensitive  to  scale,  but  that  problem  has  been  solved  and  scale  is  determined 
by  the  size  of  the  turning  radius. 

The  VWPP  has  also  been  successfully  ported  to  C/C++.  The  functionality 
and  accuracy  of  the  corresponding  Matlab  implementation  has  been  matched. 
The  speed  of  the  VWPP  is  related  to  the  number  of  threats.  For  60  threats, 
the  speed  on  a  Pentium  4  is  on  average  5  milli-seconds. 

As  part  of  determining  the  real-time  computational  requirements  of  our  ap¬ 
proach,  and  fully  understanding  the  implications  of  hardware  implementation, 
in  Phase  I  we  worked  to  demonstrate  the  algorithms  on  a  prototype  UAV.  An 
existing  prototype  computing  platform  at  Brigham  Young  University  has  been 
adapted  for  real-time  control  system  testing  and  evaluation  of  autonomous  flight 
planning  algorithms.  (See  Figure  11.)  The  physical  properties  of  the  aircraft 


Figure  11:  BYU  Prototype  UAV. 

are  shown  in  Table  3.4.3.  The  aircraft  has  a  payload  for  21bs  and  an  average 
airspeed  of  35  mph.  The  UAV  is  equipped  with  a  microcontroller  and  a  full 
suite  of  sensors.  In  addition,  a  camera  is  mounted  in  the  front  of  the  camera. 
Communication  to  the  UAV  is  accomplished  through  three  separate  RF  chan¬ 
nels.  The  first  channel  is  a  standard  70  MHz  RF  channel  for  remote  control 
flight.  The  second  channel  is  a  900  MHz  RF  modem  with  2  mile  range.  This 
channel  is  used  to  communicate  with  the  on-board  computer  and  will  be  the 
primary  channel  for  guidance,  control,  and  telemetry.  The  third  channel  is  a 


Mass  and  Inertia 

h 

0.1147 

kgm ? 

in  Cb 

Iy 

0.0576 

kgm 2 

in  Cb 

Iz 

0.1712 

kgm2 

in  Cb 

Jxy 

0.0 

kgm2 

in  Cb 

Jxz 

0.0015 

kgm2 

in  Cb 

Jyz 

0.0 

kgm 2 

in  Cb 

m 

1.56 

kg 

b 

1.4224 

m 

wing  span 

S 

0.4703 

m2 

wing  area 

AR 

4.3 

aspect  ratio 

Table  1:  Physical  Parameters  for  the  MAGICC  lab  UAV. 


2.4  GHz  channel  that  transmits  the  NTSC  signal  from  the  camera.  The  heart 
of  the  system  is  a  Rabbit  Core  Module  3100  (RCM  3100)  microcontroller  that 
runs  at  50  MHz  and  has  1  MB  of  code/data  memory.  The  system  has  22  digital 
I/O  ports,  10  analog  inputs,  4  serial  TX/RX  ports,  4  pulse  width  modulated 
outputs,  6  motor  driving  channels,  and  4  quadrature  encoder  inputs.  A  bypass 
circuit  can  be  used  to  switch  between  manual  control  and  an  on-board  autopi¬ 
lot.  The  prototype  UAV  was  used  to  test  the  trajectory  generation  algorithms 
is  equipped  with  a  full  set  of  sensors  which  have  been  integrated  on  the  UAV. 
A  list  of  the  sensors  is  shown  in  Table  3.4.3. 


State  variable 

Sensor  Type 

Noise  Variance 

Sample  Rate 

X 

1.0  s 

y 

GPS 

1.0  s 

h 

absolute  pressure 

x.x  (m) 

V 

differential  pressure 

x.x  (m/s) 

0.02  s 

a 

vane/encoder 

x.x  (deg) 

P 

vane/encoder 

x.x  (deg) 

0.02  s 

4> 

Honeywell  compass 

0.5  (deg) 

0.07  s 

e 

Honeywell  compass 

0.5  (deg) 

0.07  s 

Honeywell  compass 

1.5  (deg) 

p 

rate  gyro 

x.x  (deg/s) 

Q 

rate  gyro 

x.x  (deg/s) 

r 

rate  gyro 

x.x  (deg/s) 

Table  2:  Sensors  used  on  the  MAGICC  lab  UAV. 

The  initial  direction  was  to  employ  an  off-the-shelf  autopilot  for  the  aircraft. 
However,  the  particular  product  that  was  used  (Micropilot),  did  not  prove  to 
be  easy  to  interface  with  and  a  custom  autopilot  was  developed  instead.  The 
autopilot  has  reached  a  level  of  maturity  with  successful  demonstrations  of  alti¬ 
tude  hold,  heading  hold,  and  velocity  hold.  The  autopilot  was  integrated  with 
PDA  and  voice  interfaces  and  successfully  demonstrated  to  representatives  from 
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AFRL/MN  and  AFRL/VA.  Video  demonstrations  are  available  at 

http://www.ee.byu.edu/~beard/public/uav/video/pda.avi 
http:  / /www.ee.byu.edu/~  beard/public/uav/video/voice.avi 


3.5  Summary  of  Accomplishments  in  Phase  I 

A  Voronoi-based  path  planner  was  developed  and  extended  to  account  for  poly¬ 
gon  threat  regions  and  no-fly  zones.  The  robustness  of  trajectory  smoother  with 
respect  to  rapidly  changing  input  was  investigated  and  found  to  have  no  detri¬ 
mental  effect  on  performance.  The  computational  complexity  of  the  extended 
Voronoi  path  planner  was  analyzed  and  it  was  shown  that  for  a  reasonable  num¬ 
ber  of  threats,  the  planner  will  run  in  real-time  for  static  and  dynamic  threat 
environments.  A  12  DOF  nonlinear  model  of  the  BYU  UAV  was  developed 
in  Simulink.  The  trajectory  tracker  and  complete  waypoint  path  planner  were 
integrated  and  tested  with  the  12  DOF  aircraft  model. 

In  parallel  with  the  software  development,  an  autopilot  was  developed  using 
the  12  DOF  model  and  successfully  flight-tested  a  on  the  BYU  UAV.  A  light 
weight  (2  ounce),  low-cost,  sensor  board  and  flight  computer  for  the  was  devel¬ 
oped  for  the  BYU  UAV.  The  autopilot  was  deployed  on  the  BYU  UAV  with 
with  full  implementation  of  heading  hold,  altitude  hold,  and  velocity  hold.  The 
autopilot  with  also  integrated  with  PDA  and  voice  interfaces. 


4  Plans  for  Phase  II  and  III 

In  Phase  I,  we  showed  the  feasibility  of  the  WPP,  DTS,  and  ATT  algorithms 
for  path  planning  and  trajectory  generation  as  well  as  develop  the  requirements 
for  implementation  in  real-time  environment  for  combat  UAVs.  Porting  the 
algorithms  to  C  in  Task  2  lays  the  groundwork  for  Phase  II,  which  will  entail 
implementation,  refinement,  and  demonstration  of  these  algorithms. 

Given  a  successful  outcome  of  Phase  I  research  and  Phase  II  implementa¬ 
tion,  the  technology  proposed  here  can  be  readily  commercialized  leading  to  a 
trajectory  planning  software  package  suitable  for  acquisition  by  the  Air  Force. 
The  algorithm  codes  will  be  optimized  into  a  reliable,  user-friendly  software 
suite  that  will  be  compliant  with  the  Tactical  Control  System  to  insure  inter- 
operablity  with  the  family  of  all  present  and  future  tactical  UAVs.  Our  efforts 
to  develop  this  technology  will  be  guided  by  cooperation  with  Lockheed-Martin 
and  Northrop-Grumman. 

Beyond  military  applications,  the  market  for  technology  which  enables  ma¬ 
chines  to  respond  to  change  or  the  unexpected  is  developing.  Agricultural  and 
industrial  demand  for  mobile  autonomous  robots  is  growing.  Fuelled  by  in¬ 
creases  in  capability  and  decreases  in  price,  industrial  robot  sales  in  the  US  have 
nearly  tripled  since  1991.34  Path  planning  is  an  enabling  technology  for  service 
robots  which  automate  the  collection  of  hazardous  wastes  in  hospitals,  as  well 
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as,  safe  dismantlement  of  nuclear  and  chemical-biological  weapons.  Autonomous 
hotel  vacuum  cleaners  are  another  potential  area  for  commercialization  of  this 
technology. 

ISL  is  pursuing  two  avenues  of  commercialization.  The  first  is  to  partner 
with  either  Lockheed-Martin  or  Northrop- Grumman  to  develop  autonomous 
route-planning  software  for  future  military  UAV  platforms  such  as  LOCAAS. 

We  are  also  seeking  backing  of  TeleDyne  Controls  to  apply  this  technology 
to  future  commercial  applications.  ISL  met  with  Teledyne  Controls  of  Santa 
Monica,  CA  to  determine  their  capabilities  and  requirements  for  implementation 
of  autonomous  navigation  aids  in  flight  control  systems.  A  similar  ongoing 
discussion  is  underway  with  the  Honeywell  Corp.  Future  flight  management 
systems  will  record  and  share  environmental  information  and  weather  conditions 
in  real-time.  On-board  software  could  use  this  data  to  adjust  propulsion  and 
flight  control  systems  to  achieve  optimized  flight  profiles.  This  would  allow 
planes  to  change  their  routes  in  real-time  to  avoid  turbulence  and  take  advantage 
of  favorable  wind  conditions  while  avoiding  other  aircraft. 


5  Technology  Transitions 

BYU’s  initial  trajectory  generation  research  in  cooperative  control  work  led 
to  Phase  I  of  this  STTR.  The  Phase  I  effort  has  directly  resulted  in  several 
technology  transfer  opportunities.  Recently,  successful  transition  of  technology 
developed  in  Phase  I  to  activities  of  current  and  practical  interest  to  the  Air 
Force,  has  occurred.  For  example: 

•  AFRL/MN.  Efforts  to  fly  trajectories  on  UAV  hardware  under  Phase  I 
led  to  work  with  AFRL/MN  to  develop  miniature  autopilot  hardware 
and  software  technologies  that  were  recently  used  by  Air  Force  Special 
Operations  forces  in  war  games  in  Mississippi. 

•  NASA/Ames.  The  waypoint  path  planning  portion  of  our  trajectory 
generation  work  has  been  utilized  by  the  Army/NASA  Rotorcraft  Divi¬ 
sion  in  their  Precision  Autonomous  Landing  Adaptive  Control  Experiment 
(PALACE)  program. 

•  Spin-ofF  Company.  The  autopilot  developed  in  part  under  Phase  I  has 
generated  enough  purchasing  interest  by  university  and  government  lab 
researchers  that  a  small  company  has  been  created  to  manufacture  and 
market  autopilot  hardware  and  software. 
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1.  Erik  P.  Anderson,  Randal  W.  Beard,  “An  Algorithmic  Implementation  of 
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9  Appendix  1:  Application  to  Coordinated  Con¬ 
trol 

Future  research  on  UAVs  will  increasingly  focus  on  cooperative  control  scenar¬ 
ios  involving  multiple  vehicles.  Examples  include  cooperative  timing,  search, 
and  ISR  missions,  and  formation  flight.  Solutions  to  cooperative  control  prob¬ 
lems  will  increasingly  rely  on  real-time  path  planning  and  trajectory  generation 
techniques.  For  example,  in  cooperative  timing  problems  it  is  necessary  to 
search  through  a  set  of  paths  that  are  jointly  feasible  for  the  UAV  team.  There¬ 
fore,  computationally  efficient  techniques  for  path  planning  become  essential. 
As  another  example,  cooperative  search  and  ISR  scenarios  are  inherently  cou¬ 
pled  at  the  path/trajectory  level.  In  addition,  multiple  vehicles  in  a  congested 
battlefield  necessitate  collision  avoidance  and  deconfliction  techniques  at  the 
path/trajectory  level. 

Therefore,  computationally  efficient  real-time  path  planning  and  trajectory 
generation  techniques  directly  impact  the  state-of-the-art  in  cooperative  control. 
In  fact,  the  path  planning  and  trajectory  generation  techniques  funded  under 
Phase  I  were  originally  developed  to  facilitate  solutions  to  cooperative  timing 
problems.  In  this  regards,  a  key  feature  of  the  path  planning/trajectory  gen¬ 
eration  techniques  is  that  the  real-time  trajectories  have  the  same  path  length 
as  the  waypoint  paths,  and  thus  satisfy  the  same  timing  constraints.  The  re¬ 
sult  is  that  cooperative  timing  missions  can  be  planned  using  waypoint  paths, 
significantly  reducing  the  computational  overhead. 

Applications  of  the  techniques  developed  in  Phase  I  to  cooperative  control 
scenarios  is  reported  in.23>29-31> 35-39 
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10  Appendix  2:  Algorithm  Codes  for  12  Degree 
of  Freedom  Simulation 

The  algorithm  codes  for  the  Matlab/C  simulation  of  the  12  DOF  UAV  model 
of  the  way  point  path  planner,  trajectory  smoother,  and  tracker  are  provided 
on  a  CD  accompanying  the  report.  A  hard  copy  of  the  files  is  included  as  well. 

10.1  Matlab  Script  Files 

function  y  =  autopi lot (u,Ts, auto) 

X  autopilot 

X 

X  autopilot  function  for  MAGICC  lab  UAV 

X 

X  internal  function  states  will  be  labeled  X.name 

X 

X  Modification  History: 

X  3/28/03  -  RVB 

X 


%  inputs 


psi_c  =  u(l)  ; 
Xphi_c  =  u(i); 


V_c 

=  u(2); 

h_c 

=  u(3) ; 

X 

=  u(4) ; 

y 

=  u(5) ; 

h 

=  u(6) ; 

V 

=  u(7) ; 

alpha 

=  u(8) ; 

beta 

=  u(fl); 

phi 

=  u(10) 

theta 

=  u(U) 

psi 

=  u(12) 

P 

=  u(13) 

q 

=  u(14) 

r 

=  u(15) 

t 

=  u(16) 

X  heading  command 
X  roll  command 
X  velocity  command 
X  altitude  command 
X  inertial  x-position 
X  inertial  y-position 
X  altitude 

X  velocity  (air  speed) 

X  angle  of  attack 
X  sideslip  angle 
X  roll  angle 
X  pitch  angle 
X  heading  angle 
X  roll  rate 
X  pitch  rate 
X  yaw  rate 
X  time 


X  implement  autopilot  modes 


X  velocity  autopilot 
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delta_t  =  velocity_hold(V,  V_c,  Ts,  t) ; 


7,  longitudinal  autopilots 

if  1. 

theta_c  =  altitude_hold(h,  h_c,  Ts,  t) ; 
q_c  =  0 ; 

u  *  V*cos (alpha) *cos (beta) ; 

w  =  V*sin(alpha)*cos(beta) ; 

x  =  [u;  w;  q;  theta]  ; 

xd  =  [V_c;  0;  q_c;  theta_c] ; 

delta_e  =  longitudinal_autopilot(x,  xd,  t,  Ts,  auto); 
else 

theta.c  =  altitude_hold(h,  h_c,  Ts,  t) ; 
q_c  =  pit ch_attitude_hold (theta,  theta_c,  Ts,  t) ; 
delta_e  =  pitch_rate_hold(q,  q_c,  Ts,  t) ; 
end 

7,  lateral  autopilots 

if  1, 

phi_c  =  heading_hold(psi,  psi_c,  Ts,  t) ; 
p_c  =  0; 

x  =  [beta;  p;  r;  phi]; 

xd  -  [0;  0;  0;  phi_c] ; 

[delta^a,  delta.r]  =  lateral_autopilot (x,  xd,  t,  Ts,  auto); 
else 

phi_c  =  heading_hold(psi,  psi_c,  Ts,  t) ; 
p_c  =  roll_attitude_hold(phi,  phi„c,  Ts,  t) ; 
delta. a  =  roll.rate_hold(p,  p_c,  Ts,  t) ; 
delta.r  =  side_slip_hold(beta,  r,  Ts,  t) ; 
end 


7*  mix  aileron  and  elevator  commands 


mmmmmmmmsmssssMSSMHSssssssssssmmmmmm 


7*  control  outputs 
u  =  [.  . . 

delta_e  +  delta_a;...  7#  delta.er:  right  elevon 
delta,  e  -  delta.a; . . .  7*  delta.el:  left  elevon 
delta.r;  .  . .  7.  delta.rr:  right  rudder 

delta.r;  .  . .  7.  delta.rl:  left  rudder 

delta_t ; .  . .  7.  delta_t:  thrust 

3; 

7,  desired  states  (for  visulation  and  debugging) 
xd  =  [ . . . 

0;...  7#  x:  desired  x-position  (not  accounted  for  y  autopilot) 


0;...  •/,  y:  desired  y-position  (not  accounted  for  y  autopilot) 

h_c;...  X  h:  desired  altitude 

V_c;...  X  V:  desired  velocity 

0;...  X  alpha:  desired  angle  of  attack 

0;...  X  beta:desired  side  slip  angle 

phi_c;...  X  phi:  desired  roll  angle 
theta_c;...  X  theta:  desired  pitch  angle 
psi_c;...  7.  psi:  desired  yaw  angle 

%0;  ... 

p_c;  ...  X  desired  roll  rate 

q_c;  ...  '/.  desired  pitch  rate 

0;  . .  .  '/.  desired  yaw  rate 

]; 

X  output 
y  =  [u;  xd]  ; 


X  Autopilot  functions 


vvvvvvvvvvvvvvvvvvvvvvvvvvvY 


•/•/•/•/•/  V  V  VV  V  V  V  V  V  V  V  V  V  V  V  V  V  V  V  V  V  V  V  V  •/  •/  V.  7.  •/  •> 


X  lateral  autopilot 

xxmmmmmxxmxmxmxmxxmmxxxmxmmxxmmxmxx 

function  [delta_a,  delta.r]  =  lateral_autopilot(x,  xd,  t,  Ts, 
auto) ; 

if  isempty(auto.L_lat) ,  X  no  observer 
u  =  -auto.K_lat*(x-xd) ; 
else 

X  initialize  the  observer  state 
persistent  X; 

if  isempty(X) I (t<2*Ts) ,  X.xhat  =  zeros (4,1);  end 
X.xhat  =  auto . A_lat_d*X . xhat  +  auto .B_lat_d*(x-xd) ; 
u  =  -auto. K_lat* (X.xhat) ; 
end 


delta. a  =  u(l) ; 
delta.r  =  u(2) ; 


X  longitudinal  autopilot 
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function  delta.e  =  longitudinal .autopilot (x,  xd,  t,  Ts,  auto); 
if  isempty(auto.L_lon) ,  %  no  observer 
delta.e  =  -auto.K.lon*(x-xd) ; 
else 

*/,  initialize  the  observer  state  to  initial  xd 
persistent  X; 

if  isempty(X) | (t<2*Ts) ,  X.xhat  -  zeros(4,l);  end 
X.xhat  =  auto . A_lon_d*X . xhat  +  auto .B.lon.d*(x-xd) ; 
delta.e  =  -auto .K.lon* (X.xhat ) ; 
end 


%  altitude .hold 

%  -  regulate  altitude 

%  -  produces  desired  pitch  angle 


function  theta.c  =  altitude_hold(h,  h.c,  Ts,  t) ; 
persistent  X; 

if  isempty(X) I (t<2*Ts) ,  X. integrator  =  0;  end 


*/«  error  equation 
e.h  =  h.c  -  h; 


A  =  2; 

B  =  30; 


if  e.h  >  A, 

theta.c  =  B*pi/180; 

X. integrator  =  0; 
elseif  e.h  <  -A 

theta.c  =  -B*pi/180; 

X. integrator  =  0; 

else 

if  abs(e.h)  >  0.5*A, 

X. integrator  =  0; 

else 

*/,  update  integrator 
X. integrator  =  X. integrator  +  Ts*e.h; 
end 

*/,  implement  PI  control 
kp  =  B*pi/180/A; 

ki  =  .01; 

theta.c  =  (kp  *  e.h  +  ki  *  X . integrator) ; 
end 


X  pitch_attitude  Jiold 
X  -  regulate  pit ch_ attitude 
X  -  produces  desired  pitch  rate 

mxmmxmmmmxmxmmxmxxmmmmmmmmmwc 

function  q_c  =  pitch_attitude_hold (theta,  theta_c,  Ts,  t); 
persistent  X; 
if  isempty(X) I (t<2*Ts) , 

X. integrator  =  0; 

X.theta_prev  =  0; 

X.theta.f iltered_prev  =  0; 

end 


X  implement  digital  washout  filter 
Kw  =  .01;  X  washout  filter  gain 

X  Kw  =  0;  X  washout  filter  gain 

kwl  =  1/ (l+(Ts*Kw/2)) ; 
kw2  =  ( 1- (Ts*Kw/2) ) *kwl ; 

theta_f iltered  -  kw2  *  X.theta_filtered„prev. . . 
+  kwl  *  (theta  -  X. theta_prev) ; 

X  error  equation 

e_theta  =  theta_c  -  theta.f iltered; 

X  update  persistent  variables 
X. integrator  =  X. integrator  +  Ts*e_theta; 
X.theta_f iltered_prev  =  theta_f iltered; 
X.theta.prev  =  theta; 

X  implement  PI  control 
kp  =  10; 

ki  =5; 

q_c  =  kp  *  e_.th.eta  +  ki  *  X. integrator; 

X  saturate  pitch  rate 

q_c  =  sat(q_c,  100*pi/180,  -100*pi/180) ; 


•/vvvvvvvvvvvvvvvvvvvvvvvvvv.v*m//*////m*///*/V/y 


X  pitch_rate_hold 


X 

*/*/•/•/•/•/•> 


regulate  pitch_rate 

*/•/  */•/  •/«/  •/  •/  •/  •/  •/  •/  v  •/•/  •/  •/  v  v  •/  v  v  */  •/  */  v  v  v  v  v  v  •/•/•/•/•/' •/•'  •/ #/  •/  */#/  #/  •/  v  •/  •/  •/  v  */  v  v  v  v  v  v  v  v  v  v  v 


function  delta_e  =  pitch_rate_hold(q,  q_c,  Ts,  t) ; 
persistent  X; 

if  isempty(X) I (t<2*Ts) ,  X. integrator  =  0;  end 


X  error  equation 
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e.q  =  q_c  -  q; 


%  update  integrator 

X. integrator  =  X. integrator  +  Ts*e.q; 

*/,  implement  P  control 
kp  =0.65; 

ki  =1; 

delta.e  =  kp  *  e_q  +  ki  *  X. integrator ; 

*/#  saturate  the  output 

delta. e  =  -sat (delta.e,  45*pi/180,  -45*pi/180  ); 


•/,  heading  hold 
7,  -  regulate  heading  angle 
%  -  produces  desired  roll  angle 


function  phi.c  =  heading_hold(psi ,  psi_c,  Ts,  t) ; 


persistent  X; 

if  isempty(X) I (t<2*Ts) ,  X. integrator  =  0;  end 


7,  error  equation 
e.psi  =  psi.c  -  psi; 


A  =  10; 
B  =  20; 


if  e.psi  >  A*pi/180, 
phi.c  =  B*pi/180; 

X. integrator  =  0; 
elseif  e.psi  <  -A*pi/180, 
phi.c  =  -B*pi/180; 

X. integrator  =  0; 

else 

if  abs(e.psi)  >  A/5*pi/180, 

X. integrator  =  0; 

else 

%  update  integrator 

X. integrator  =  X. integrator  +  Ts*e.psi; 
end 

%  implement  PI  control 
kp  =  B/A; 

ki  =0; 
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phi_c  =  kp  *  e_psi  +  ki  *  X. integrator ; 
end 


•/■/•/•/*/•/•/•/*/•/•> 


7,  ro  11  _  attitude  .hold 
*/#  -  regulate  roll_attitude 

7,  -  produces  desired  roll  rate 

•/•/•/•/•/•/■/VVV./VVVVVVVVVVVVV  •/•/•/•/•/•/•/'/•/•/•/•/V'/V 


function  p_c  =  roll_attitude_hold(phi ,  phi_c,  Ts,  t)  ; 
persistent  X; 

if  isempty(X) I (t<2*Ts) ,  X. integrator  =  0;  end 


7.  error  equation 
e_phi  =  phi_c  -  phi; 

A  *  3; 

B  =  10; 


if  e_phi  >  A*pi/180, 
p_c  =  B*pi/180; 

X. integrator  =  0; 
elseif  e_phi  <  -A*pi/180, 
p_c  =  -B*pi/180; 

X. integrator  *  0; 

else 

if  abs(e_phi)  >  A/5*pi/180, 

X. integrator  =  0; 

else 

%  update  integrator 

X. integrator  =  X. integrator  +  Ts*e_phi; 
end 

7,  implement  PI  control 
kp  =  B/A; 

ki  -  0; 

p_c  =  kp  *  e_phi  +  ki  *  X. integrator ; 
end 


•/  •/  •/  */  •/  •/  •/  •/  v  •/  •/  •/  v  •/  v  v  v  v  v  •/  v  v  v  v  v  v  v  v  v  v  v  */ 


7,  roll_rate_hold 
%  -  regulate  roll_rate 


function  delta.a  =  roll_rate_hold(p,  p_c,  Ts,  t) ; 
persistent  X; 

if  isempty(X) I (t<2*Ts) ,  X. integrator  =  0;  end 
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%  error  equation 
e_p  =  p_c  -  p; 

7,  update  integrator 

X. integrator  =  X. integrator  +  Ts*e_p; 

7*  implement  P  control 
kp  “5; 

ki  =  2; 

delta.a  ■  kp  *  e.p  +  ki  *  X. integrator ; 

delta. 1  =  p_c  -  kp*p; 

%  saturate  the  output 

delta. a  =  -sat (delta.a,  30*pi/180,  -30*pi/180  ); 


%  side.slip.hold 

7,  -  hold  the  side  slip  angle  to  zero 


function  delta.r  =  side_slip_hold(beta,  r,  Ts,  t) ; 
persistent  X; 

if  isempty(X) I (t<2*Ts) ,  X. integrator  «  0;  end 


7,  update  integrator 

X. integrator  =  X. integrator  +  Ts*beta; 

%  implement  PI  control 
kp  =  20; 

ki  =  1 ; 

kd  =  -1 ; 

delta.r  =  kp  *  beta  +  ki  *  X. integrator  +  kd*r; 

7.  saturate  the  output 

delta.r  =  -sat (delta.r,  10*pi/180,  -10*pi/180  ); 


%  velocity.hold  -  PI  control  to  hold  velocity  using  throttle  only 

vvvvvvvvvvvvvvvvvvyvvvvmvvvMy.y.y.mYmrnrammmmmxmm 


function  delta. t  =  velocity_hold(V,  V_c,  Ts,  t) ; 
persistent  X; 


%  convert  inputs  to  general  variables 
y  =  V; 
r  =  V.c; 
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•/,  controller  constants 

am  =  .5;  %  approximately  1.5  second  rise  time 
bin  =  am; 
garni  =  1; 
gam2  =  1; 

if  isempty(X) I (t<2*Ts) , 

X.ym  =  r; 

X.thl  =  bm/am; 

X.th2  =  1/am; 

end 

*/,  output  control 

u  =  X.thl*y  +  X.th2*(am*r  -  bm*y  +  garni * (X. ym-y) ) ; 

%  update  state  equations 

X.ym  =  X.ym  +  Ts*(  -bm*X.ym  +  am*r  ); 

X.thl  =  X.thl  +  Ts*(  garni *y*(X.ym-y)  ); 

X.th2  =  X.th2  +  Ts*(  (X.ym-y)*(am*r  -  bm*y  +  garni*  (X.ym-y))  ); 


7,  saturate  the  output 
delta_t  »  sat(u,  10,  -10  ); 


•/•/•/•/•/•/vvvvvvvvvvvvvvv 


7.  sat 

7*  -  saturation  function 


function  out  =  sat(in,  up_limit,  low_limit) ; 
if  in  >  up_limit, 
out  =  up_limit; 
elseif  in  <  low_limit; 
out  =  low^limit; 


else 

out  =  in; 

end 

function  ic  =  calcInitialCond; 

global  P; 

global  uavdata; 

global  env; 

global  M; 


for  p=l :P, 

uav (p)  . path  =  []  ;  7.  3  x  n  [path  [x;  y;  h]] 
uav(p).tg  =  0; 
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uav(p).st  =0; 

startpts  =  [reshape (uavdata(p) .ic (1:2) ,1,2) ;  env. targets (:,: ,p)] ;  7*  each  row 
while  size (uav (p) .path, 2)  <  3, 

if  uav(p).tg+l  >  size (env. targets (: , : ,p) ,1) , 
uav(p) .tg  =  1; 
else , 

uav(p).tg  -  uav(p).tg  +  1; 
end; 

if  uav(p).st+l  >  size (startpts, 1) , 
uav(p) .st  =  2; 
else, 

uav(p).st  =  uav(p).st  +  1; 
end; 


vel  =  (uavdata(p)  ,v_max  +  uavdata(p)  .v__min)/2;  %  pick  velocity  inbetween  min  and  ma: 
[vp,  vel]  =  generateVPpath(env. threats,  startpts (uav (p) .st, :) ,  env. targets (uav(p)  .tj 
pathlen  =  size(vp,2); 

uav(p) .path  *  [  uav (p) .path  [vp(2:3,:);  uavdata(p) . ic(5)*ones(l , pathlen)]  ]; 
uav (p). vel  =  vel; 
end; 
end; 


mxxxxmxmmxxxx  target  manager  xmxmmxxxmmmxxx 


%  initial  targets 

targets  =  env. targets (uav(l) .tg, :, 1) * ; 
for  p=2:P, 

targets  =  [targets;  env. targets (uav (p) .tg, : ,p) 0 ; 
end 


7.  initialize  target  manager  state 
ic.targetmgr_init  =  [... 

ones(P,l)  ; .  . .  7.  initial  state  of  state  machine 

targets;...  %  initial  target  locations 

ones (P,  1)  ; . . .  7.  initial  target  pointers 

]; 

ic.targetmgr_in  =  zeros (P,l); 


wpp 


ic.wpp^init  =  [. . . 

size (env. threats, 1) ; .  .  . 
ones(P,l) ; . . . 


]; 


%%%%%%%%%%%%%%%%%%%%%%%%%% 


7#  number  of  threats 

7.  initial  state  of  state  machine 


for  p=l:P, 

[m,  n]  =  size (uav (p) .path) ; 
if  m*n  >  3*M, 

disp(,Must  increase  M  to  at  least: 
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m*n 


end; 

uav(p) .padded.path  =  [reshape (uav(p) .path,m*n, 1) ;  zeros(3*M  -  m*n,l)] 
uav(p) .pathlen  =  n; 
ic.wpp_init  =  [ic . wpp_init ;  ... 

n-  X  number  of  wps  in  path 

uav(p)  .  vel;  ...  X  velocity  along  path 

uav(p)  .padded_path;  ...  X  path  with  zeros 

3; 


end; 

ic.wpp_init  =  [ic.wpp_init ;  ... 

targets;  ...  X  initial  targets 
zeros (P,l)  ;  ■  •  .  '/.  initial  flag  value 


]; 


XXXXXXXXXXXXXXXXXXXXX  waypoint  mng 


ic.wpmng_init(p) .xO  =  [  ... 
1;... 

0;... 

uavdata (p) . ic ( 1 : 3) ; . . . 
1;... 

uav(p) .padded_path;  ... 
zeros(3*M,l) ;  ... 
size(env. threat s,l) ; . . . 


0;... 

uav(p) .pathlen; . . . 
uav(p) . padded.path ; . . . 
uav(p) .pathlen; . . . 

3; 

end; 


xxxxxxxxxxxxxxxmxmxxw. 


X  state ; . . . 

X  request_new_path; . . . 

X  w0;... 

X  wpPtr; . . . 

X  wpQueue ; . . . 

X  num_threats_prev; . . . 
X  counter 
X  path_l en 
X  stored_path 
X  stored_path_len 


for  p=l:P, 

ic. wpmng.in(p) . inO  =  [  ... 

uav(p) .pathlen;  . . . 
uav(p).vel;  ... 
uav(p) .padded_path;  . . . 

3; 

end; 


xxxxxxxxxxxxxxxxxxx 7.7.  traj  Gen  xxxxxxxxxxxxxxxxxxxxxxxxxx 


for  p=l:p, 

ic.trajGen_in(p) . inO  =  [uav(p).vel;  uav(p) .padded_path(l :9)] ; 
uavdata (p) .ic(4)  =uav(p).vel; 

end; 

function  [sys,x0,str ,ts]  =  constrained_tt (t ,x,u, flag, uavdata ,P) 
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X  s-f unction  implementation  of  adaptive  trajectory  tracker 
*/,  without  adaptation. 

X 

X  Modified:  9/26/02  -  RWB 

X 


switch  flag. 


xxxxxxxxxxxxxxxxxx 

%  Initialization  */, 

XXXXXXXXXXXXXXXXXX 

case  0, 

[sys,xO,str,ts]=mdlInitializeSizes(P) ; 


xxxxxxxxxxx 

%  Outputs  X 

xxxxxxxxxxx 


case  3, 

sys^mdlOutputs (t ,x,u,uavdata,P) ; 


xxxxxxxxxxxxxxxxxxx 

X  Unhandled  flags  '/« 

XXXXXXXXXXXXXXXXXXX 

case  {  1,  2,  4,  9  >, 
sys  =  []  ; 


xxxxxxxxxxxxxxxxxxxx 

X  Unexpected  flags  X 

XXXXXXXXXXXXXXXXXXXX 


otherwise 

error ( [* Unhandled  flag  = 


1  ,num2str (f lag)]  ) ; 


end 

X  end  csfunc 


X 

X-— — 

X  mdllnitializeSizes 
X  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-function 


function  [sys,xO,str ,ts] =mdl!nitializeSizes (P) 


sizes  =  simsizes;  sizes .NumCont States  =  0;  sizes .NumDiscStates  = 
0;  sizes . NumOutputs  =  3*P;  sizes .Numlnputs  —  11*P; 

sizes .DirFeedthrough  =  1;  sizes .NumSampleTimes  =  1; 
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sys  =  simsizes (sizes) ;  xO  =  []  ;  str  -  []  ;  ts  -[0  0]; 
%  end  mdllnitializeSizes 


% 

•/,  mdlOutputs 

%  Return  the  block  outputs. 


function  yy=mdlOutputs (t ,xx,u,uavdata,P,  tparam)  yy  =  [] ; 
%  interpret  states  and  inputs 
for  p=l:P, 

eps_vmin  =  uavdata(p) .track_eps_vmin; 
eps_vmax  =  uavdata(p) .tracks eps_vmax; 
eps_wmax  =  uavdata(p) .track_eps_wmax; 
vmin  =  uavdata(p) . v_min  -  eps_vmin; 
vmax  =  uavdata(p) . v_max  +  eps_vmax; 
wmax  =  uavdata(p) .psidot_max  +  eps_wmax ; 


x_r  =  u(l+ll*(p-l)) ; 

y_r  =  u(2+ll*(p-l)) ; 

psi_r  =  u(3+ll*(p-l)) ; 

V_r  =  sat (u (4+11* (p-1))  ,vmax-eps_vmax,vmin+eps_vmin)  ; 

w_r  =  sat  (u(5+ll*(p-l)) ,wmax-eps_wmax ,-wmax+eps_wmax) ; 

x  =  u(6+ll*(p-l)) ; 

y  *  u(7+ll*(p-l)) ; 

psi  *=  u(8+ll*  (p-1) )  ; 

V  =  u(9+ll*(p~D); 

h  =  u(10+ll*(p-l)) ; 

hdot  =  u(ll+ll*(p-l)); 


cp  =  cos (psi); 
sp  =  sin(psi) ; 

xO  =  angle_diff(  psi_r,  psi  ); 
xl  =  - sp* (  x_r  -  x  )  +  cp*(  y_r  -  y  ); 
x2  =  -cp*(  x_r  -  x  )  -  sp*(  y_r  -  y  ); 
cxO  =  cos(xO); 


XxObar  =  100.0*x0+xl/sqrt (l+xl*xl+x2*x2) ; 
xObar  =  xO+xl/sqrt (l+xl*xl+x2*x2) ; 


ul_up  =  vmax  -  V_r*cxO; 
ul_low  =  vmin  -  V_r*cxO; 
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uO  =  -sat (10*x0bar , eps_wmax , -eps_wmax) ; 
ul  =  sat(-10*x2,ul_up,ul_low) ; 

%  compute  actual  controls 
7.v  =  w_r ; 

7.v  =  V_r ; 
v  =  w_r  -  uO; 
v  =  V_r*cxO  +  ul; 

psi_c  =  psi  +  w*uavdata(p) .tau_psi ; 

7,  command  roll  angle 
7,psi_c  =  atan2(w*v,  9.80665); 

yy  =  Cyy;  psi_c;  v;  w] ; 

end; 

function  nval  =  sat(val,  mx,  mn) 
if  val  >  mx,  nval  =  mx; 
elseif  val  <  mn,  nval  =  mn; 
else,  nval  =  val;  end; 


function  err  =  angle_dif f (aref ,  areal) 
err  =  aref  -  areal; 

while  err  >  pi,  err  =  err  -  2*pi;  end; 

while  err  <  -pi,  err  =  err  +  2*pi;  end; 7*7#  dofl2param.m 

7.7. 

7.7.  Parameter  file  for  MAGICC  lab  UAV 


7.7. 

7.7.  Modified: 

7.7.  10/31/02  -  RWB 


7.  initial  state 

dof 12simulator (p) .x0  =  [. . . 


uavdata(p) .ic(l) ; . . . 

*/.  X: 

uavdata(p) . ic(2) ; . . . 

%  Y: 

uavdata(p) .ic(5) ; . . . 

•/.  h: 

uavdata(p) .ic(4) ; . . . 

V.  V: 

2 .4*pi/180; . . . 

'/,  alpha: 

0;... 

'/,  beta: 

0;... 

7.  phi: 

2 ,4*pi/180; . . . 

7,  theta: 

uavdata(p) . ic(3) ; . . . 

7.  psi: 

x-position  (m) 
y-position  (m) 
altitude  (m) 
velocity  (m/s) 
angle  of  attack  (rad) 
sideslip  angle  (rad) 
roll  angle 
pitch  angle 
yaw  angle 
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0;... 

7.  p: 

roll  rate 

0;... 

7.  q: 

pitch  rate 

0;... 

7.  r: 

yaw  rate 

]; 

end; 


inches2meters  - 

X  items  we  need 

C.ps^inches 

C.ps 

C.T_F 

C.T 


2.54/100; 

to  know  to  calculate  dynamic  pressure 

=  30.02;  */•  air  pressure  in  inches-Hg 

=  C.ps_inches*100*1.01e5/39.4/76;  7.  (N/m'2)  air  pressure 

-  j  X  Temperature  in  Fharenheit 

=  5/9*  (C .  T  JP-32) +273 . 15 ;  7.  Temperature  in  Kelvin 


7.  Physical  parameters  of  aircraft 


C . wingspan 
C.root_chord 
C.tip^chord 
C . angle_of .sweep 
C.m 

C.wing^loading 

C.wing_area 

C.cg 

C . required_washout 
C . stability_f actor 
C.K1 
C.K2 

C.J  -  [... 


1.4224;  X  (meters)  =  56  inches  *2.54/100 
0.4572;  7,  (meters)  =  18  inches  *2.54/100 
0.2032;  7*  (meters)  =  8  inches  *2.54/100 

0.6632;  7.  (rad)  =  38  degrees  *pi/180 

1.56;  7,  (kg)  mass  =  55  ounces  *28.35/1000 

3.325;  7,  kg/nT2  =  10.9  ounces/ft "2  *28. 35/1000*3. 28~2 

0.4703;  7.  (m~ 2)  =5.06  ft~2  /3. 28/3. 28 

0.3089;  X  (m)  center  of  gravity  behind  LE  at  root 
0.01506;  X  (m)  Tip  TE  dist  rise  from  LE 


=  0.043; 

7.  ?? 

=  0.622; 

7.  ?? 

=  0.378; 

7.  ?? 

X  inertia  matrix  in  kg-nT2  (rough  estimate) 


12.2,  0,  0.16;... 
0,  6.12,  0;... 
0.16,  0,  18.2;... 
]*(14. 6/(39. 4~2)); 


X  Airfoil  specs 

C.airfoil_type 

C . root_zero_lif t.angle 

C . root_pitch_moment 

C . t ip_zero_lif t_angle 

C . design_lif t_coef 

C.mean_chord 

C . mean_chord_winglet 

0.444; 

C . alpha_total 
C . alpha_modif ied 


* EH2010/EH2012 i ; 

0.0742*pi/180;  7.  (radians)  =  0.0742  degrees  *pi/180 

0.0001; 

0.0743*pi/180;  7.  (radians)  =  0.0743  degrees  *pi/180 
0.4255;  C. aspect _ratio  =  4.3; 

0.3302;  */,  (meters)  =  13  inches  *2.54/100 

6*inches2meters;  C.taper_ratio 

-0.0733;  7.  (radians)  =  -4.2  degrees  *pi/180 

-0.0733;  7.  (radians)  =  -4.2  degrees  *pi/180 
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C.aerodynamic_center  =  0.3226; 
C.S  =  C.wing.area; 

C.S_elevon  =  0.03; 

C . elevon_mean_chord  =  0.0549; 


(meters)  =  12.7  inches  behind  LE  at  root 

%  (nT 2)  =46.5  in"2 
•/.  (m)  =  2.16  in 


*/*  flying  properties 

C.v.ave  =  11.18;  */,  (m/s)  =  25  miles/hr*l .  16*1000/60/60 


*/,  physical  parameters 


c.g 

C . lambda 

C.Ra 

C.pO 

C.R 

C.MO 

C.Rearth 

C.rho 


=  9.80665 
=  -0.0065 
=  8314.32 
=  101325; 

=  287.05; 

=  28.9644; 

«  6371020; 

=  C.ps/C.R/C 


%  (m/s" 2)  gravitational  constant 

%  (Kelvin/m)  temperature  gradient  in  troposphere 

*/,  (J/K/kmol)  molar  gas  constant 

*/,  (N/m)  air  pressure  at  sea  level 

*/,(J/K/kg)  specific  gas  constant 

*/,  (kg/kmole)  molecular  weight  of  air  at  sea  level 
•/,  (m)  radius  of  earth 
T;  %  (kg/m"3)  air  density 


'/,  aerodynamic  coefficients 
C.b  =  C. wingspan; 

C.cbar  =  C  .mean__chord; 


%  coefficients  from  the  airframe 
C.bw  =  56*inches2meters; 

C.aw  =  C.bw/2*sin(38*pi/180) ; 

C.cw  =  10.66*inches2meters; 

C.dw  =  18*inches2meters; 

xw  =  max (roots ([(C.cw-C.dw)/(C.bw/2),  -C.dw*C.bw/2,  - (C . cw-C . dw) *C . bw/4] ) ) ; 

C.bwl  =  12*inches2meters ; 

C.cwl  =  4*inches2meters; 

C . dwl  =  8*inches2meters ; 

xwl  =  max(roots([(C.cwl-C.dwl)/(C.bwl/2),  -C . dwl*C . bwl/2 ,  -(C.cwl-C.dwl)*C.bwl/4])> ; 

C.R_cg  «  [-12 . 16*inches2meters ;  0;  0];  */,  center  of  gravity  wrt  leading  edge 

acw  =  12.7*inches2meters; 

acwl  =  xwl*sin(38*pi/180)  +  (C.dwl  +  (C.cwl-C. dwl) /(C. bwl/2) *xwl)/4; 

C . R_ac  =  [-acw;  0;  0]  ;  */.  aerodynamic  center  wrt  leading  edge 

C . R_rw  =  C.R_ac  +  [0;  xw;  0]; 

C.R.lw  =  C.R_ac  +  [0;  -xw;  0]; 

C.R_rwl  =  [-C.aw-acwl;  C.bw/2;  -xwl]; 

C.R_lwl  =  [-C.aw-acwl;  -C.bw/2;  -xwl]; 

aerodynamics  coefficient  for  force  model  (From  ModelFoil  Version  4.1) 

*/,  wing  coefficients 
C.C_Lw_0  =  0; 

C.C_Lw_alpha  =  0. 1032* (180/pi) ; 

C.C.Lw.delta  =  0. 0509* ( 180/pi) ; 
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C.CLDw.O  =  0.0130; 

C . C_Dw_K  =  -0.0032; 

C . C_Mw_0  =  0; 

C . C_Mw_alpha  =  -0. 0016* (180/pi) ; 

C.C_Mw_delta  =  -0. 0117* (180/pi) ; 

C.C_l_p  =  -1;  */.  moment  due  to  roll  rate 

C.C_m_q  =  -.5;  7,  moment  due  to  pitch  rate 

'/.winglet  coefficients 
C . C_Lwl_0  =  0; 

C . C_Lwl_alpha  =  0. 1032* (180/pi) ; 

C.C_Lwl_delta  =  0. 0509* (180/pi) ; 

C.C_Dwl_0  =  0.0130; 

C.C_Dwl_K  =  -0.0032; 

C . C_Mwl_0  =  0; 

C . C_Mwl_ alpha  =  -0. 0016* (180/pi) ; 

C.C.Mwl.delta  =  0.0117*(180/pi) ; 

7,  engine  coefficient 
C.C_engine  =  1; 

'/,  aerodynamic  coefficents  for  linear  force  model 
’/,  longitudinal  coefficents 
C.C_X_0  =  -0.03554; 

C . C_X_alpha  =  0.002920; 

C . C_X_q  =  -0.6748; 

C.C_X_elevator  =  0.03412; 

C . C_X_throttle  =  1; 

C.C_Z_0  =  -0.05504; 

C . C_Z_ alpha  =  -5.578; 

C.C_Z_q  =  -2.988; 

C.C_Z_elevator  =  -0.3980; 

C . C_Z_throttle  =  0; 

C . C_M_0  =  0.09448; 

C . C_M_ alpha  =  -0.6028; 

C . C_M_q  =  -15.56; 

C . C_M_elevator  =  -1.921; 

C . C_M_throttle  =  0; 

'/,  longitudinal  coefficents 
C . C_Y_0  =  -0.002226; 

C.C_Y_beta  =  -0.7678; 

C . C_Y_p  =  -0.1240; 

C . C_Y_r  =  0.3666; 

C.C_Y_aileron  =  -0.02956; 

C.C_Y_rudder  =  0.1158; 
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C.CLL_0 

C.CLL_beta 

C . C_L_p 

C . C_L_r 

C.C_L_aileron 

C . C_L_rudder 

C.CJJ_0 

C.CJLbeta 

C.C_N_p 

C.CJLr 

C.CJLaileron 

C . C_N_rudder 


«  0.0005910; 
-  -0.06180; 

=  -0.5045; 

-  0.1695; 

=  -0.09917; 

-  0.006934; 

=  -0.003117; 
=  0.006719; 

=  -0.1585; 

=  -0.1112; 

=  -0.003872; 
=  -0.08265; 


auto  =  my_autopilot_design((uavdata(p) .v_max  +  uavdata(p) .v_min)/2,0,C,Ts) ; 
function  [sys,xO,str,ts]  =  environment (t ,x,u, flag, env,P) 

'/♦environment 

7,  Simulate  the  environment,  allowing  for  pop-up  threats,  moving 
7,  threats  and  moving  targets. 

7. 

7. 

7,  modified  8/3/01  -  Randy  Beard 


switch  flag, 


xxxxxxxxxxxxxxxxxx 

7*  Initialization  7# 

xxxxxxxxxxxxxxxxxx 

case  0, 

[sys,x0,str ,ts]  = 


mdl!nitializeSizes(env,P) ; 


xxxxxxxxxx 

X  Update  7. 

xxxxxxxxxx 

case  2, 

sys  =  mdlUpdate(t ,x,u,env,P) ; 


xxxxxxxxxx 

X  Output  X 

xxxxxxxxxx 

case  3, 

sys  =  mdl0utputs(t ,x,u,env,P) ; 

xxxxxxxxxxxxx 

X  Terminate  X 

XXXXXXXXXXXXX 

case  9, 
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sys  =  []  ;  X  do  nothing 


xxxxxxxxxxxxxxxxxxxx 

X  Unexpected  flags  '/« 

xxxxxxxxxxxxxxxxxxxx 

otherwise 

error ( [’unhandled  flag 


*  ,num2str (flag)] ) ; 


end 

%end  dsfunc 


X  _ _ 

*/,  mdllnitializeSizes 

%  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-f unction. 


function  [sys,xO,str,ts]  =  mdllnitializeSizes (env,P) 


'/, input  ( ’  In  environment 
sizes  =  simsizes; 
sizes. NumContStates  = 
sizes. NumDiscStates  = 
sizes.NumOutputs  = 

sizes.Numlnputs  * 

sizes.DirFeedthrough  = 
sizes .NumSampleTimes  = 


init’) ; 

0; 

l+2*size (env . threats , 1) +2*size (env . popup , 1) ; 

l+2*size (env . threats , 1) +2*size (env . popup , 1) ; 

3*P; 

l; 

l; 


sys  =  simsizes (sizes) ; 

x0  =  [size (env . threats , 1) ;  reshape (env. threat s,2*size (env. threats, 1) ,1) ;  .. 

reshape (env. popup, 2*size (env. popup, 1) , 1)] ; 
str  *  []  ; 
ts  =  [0  0]; 

*/,  end  mdllnitializeSizes 

X  _ 

X  mdlUpdate 

*/,  Handle  discrete  state  updates,  sample  time  hits,  and  major  time  step 
X  requirements. 

X . 

fimction  xup  =  mdlUpdate (t ,x,u, env, P) 
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•/.input (* In  environment  update*); 
num„threats  =  x(l); 

num.popup  =  size (env. threats , l)+size (env. popup, l)-num_t hr eats; 
threats  =  reshape (x (2 : l+2*num_thr eats) ,num_threats ,2) ; 
popup  =  reshape (x(2+2*num_threats : length (x)) ,num_popup,2) ; 

for  i=l:P, 

uav(:,i)  =  u(l+3*(i-l) :2+3*(i-l)) ; 
end 


if  num_popup>0 , 
for  i=l:P, 

X  compute  the  closest  pop-up  threat  to  each  uav 
tmp=repmat(uav([l:2] ,i) ,1 , size (popup, 1)) -popup* ; 
dist  «  sqrt (diag(tmp**tmp) ) ; 

[closest .index]  =  min(dist) ; 

y.  if  a  popup  threat  is  close  to  the  uav ,  then  add  that 
y,  threat  to  the  threat  list . 
if  closest  <  5, 

num_threats  =  num_threats+l ; 

threats  =  [threats;  popup (index, :)] ; 

popup  =  [popup(l : index-1 , : ) ;  popup ( index+1 :num_popup, : )] 

num_popup  =  num_popup-l ; 

end 
end 
end 

xup  =  [num_threats;  re shape (thr eats, 2*num_thr eat s, 1) ;  ... 
reshape (popup , 2*num_popup , 1) ] ; 

•/.end  mdlUpdate 

y. 

'/,  mdlOutputs 

•/,  Return  Return  the  output  vector  for  the  S-function 
•/==========^======^============-=====-=====— ====-=====-====— ======== 

y. 

function  y  =  mdlOutputs (t ,x,u,env,P) 

•/.input  ( *  In  environment  output  * )  ; 

num_ threats  =  x(l); 

threats  =  x (2: 1+2 *num_ threats ) ; 
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%end  mdlOutput 


function  [vp,  vel]  =  generateVPpath (threats ,  st,  tg,  v) 

vp  =  fixVP (calcVP (threats,  st,  tg));  ’/.FIX  add  smoother  here 
[vp,  vel]  =  fixPath(vp,  v) ; 


%  fixVP 

This  function  removes  nodes  that  have  been  added  by  the  calcVP 
X  step,  when  required.  The  calcVP  module  was  written  under  the  assumption 
X  that  the  airplane  is  not  on  a  Voronoi  point.  If  it  is,  then  the 
%  function  adds  nodes  that  shouldn't  be  there.  This  function  removes  them. 

X— — — — - — 

function  vp  =  fixVP (vp_in) 

n=size(vp_in,2) ; 

if  0, 

if  norm(vp_in(2:3, l)-vp_in(2:3,2))<.01 , 
vp  =  vp_in(: ,2:n) ; 
else 

vp  =  vp_in; 
end 
end 

if  norm (vp_in (2 : 3 , 1)  -vp_in(2 : 3 , 3)  )  < .  01 , 
vp  =  vp_in( : ,3:n) ; 
else 

vp  =  vp_in; 
end 


X 

% 

X 

^==s==™==™====™=™====-===™=~=======~=========- 

X  calc_mult_VP 

X  Calculates  a  Voronoi-diagram-based  path  from  the  start  point  to  the 
X  target  avoiding  threats 


% 

•/♦function  [vp,pvec]  =  calc jmult_VP (threats, st ,tg)  ; 
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function  [vp,pvec]  =  calcVP (threats , st ,tg) ; 

tx  =  threats ( : ,  1) ;  ty  =  threats( : ,2) ;  [vx,vy]  =  voronoi(tx,ty) ; 

nodes  =  f ind_nodes (vx, vy) ;  [nm,nn]  =  size(nodes);  nodes_aug  = 

[nodes (1,:)  nn+1  nn+2;  . 

st (1)  nodes(2,:)  tg(l);  ... 
st(2)  nodes(3,:)  tg(2)] ; 

sn_ind  —  get_closest (nodes, st) ;  tn_ind  -  get— closest (nodes, tg) ; 

7,  Calculate  a  threat  cost  for  each  edge  of  the  Voronoi  diagram 
threat.cost  = 

calc_threat _cost (vx , vy , tx , ty , nodes , st , sn_ind , tg , tn_ind) ; 

7,Kt  is  the  priority  between  a  safe  path  and  a  long  path 

7,the  number  is  0<Kt<l.  A  number  closer  to  1  implies  a  long,  safe  path 

•/.while  a  number  closer  to  0  implies  a  shorter,  more  risky  path 

Kt=.3; 

•/.Kt =0.001; 

A  =  gen_cost_mat(vx, vy, nodes, threat_cost,st ,sn_ind,tg,tn_ind,Kt) ; 

•/.numVP  is  the  number  of  paths 
numVP  =  1; 

*/,  Use  bidirectional  arcs.  Change  cost  matrix  to  add  links  in 
%  both  directions. 

[M,N]  =  size (A) ;  D  =  -ones(M,N);  for  i=l:M, 
for  j=l:N, 

if  (A(i,jr=Inf)&  (A(i,j)-'=0), 

D(i,j)  =  A(i, j) ; 

D(j,i)  =  A(i, j) ; 
end 

end 

end  D(M,:)=-1; 

%  Bidirectional  arcs  graph  search  called  using  D  matrix 

[VP, Cost VP]  =  multipathepp(numVP,l,nn+2,D) ;  [tmp,nVP]  =size(VP); 

7,  Bidirectional  arcs  graph  search  called  using  D  matrix 
[VP,CostVP]  =  mult ipathepp (numVP, 1, nn+2, D) ;  [tmp,nVP]  =size(VP); 

[tmp,pvec]  =  min (abs (VP- (nn+2))) ;  nl  =  [] ;  for  i  =  l:nVP, 
nl.tmp  =  VP(l:pvec(i) ,i) * ; 
nl  =  [nl  nl_tmp] ; 

end 
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vp  =  nodes_aug( : ,nl) ; 
if  0,  numVP  =1; 

7,  Use  bidirectional  arcs.  Change  cost  matrix  to  add  links  in 
7,  both  directions. 

[M,N]  =  size(A);  D  =  -ones(M,N);  for  i=l:M, 
for 

if  (A(i, j) ~=Inf )&  (A(i , j)~=0) , 

D(i , j)  =  A(i , j) ; 

D(j,i)  =  A(i , j) ; 
end 

end 

end  D(M, :)=-l; 

7#  Unidirectional  arcs  graph  search  called  using  D  matrix 
[VP, Co st VP]  =  Mult iPath (numVP , 1 , nn+2 , D) ; 

7,  Unidirectional  arcs  graph  search  called  using  A  matrix 
7,  This  .m-file  is  saved  as  Mult  iPath.  m.  SAVE 
7#  [VP,  Cost  VP]  *  Mul  t  iPath  (numVP,  1,  nn+2.  A); 

[tmp,nVP]  =  size (VP); 

[tmp.pvec]  =  min(abs (VP-1) )  ;  nl  =  []  ;  for  i  =  l:nVP, 
nl_tmp  =  VP(pvec(i) :-l: l,i) * \ 
nl  =  [nl  nl.tmp] ; 

end 


vp  =  nodes_aug( : ,nl) ;  end 

7. 

- - - - - - - 

7,  find_nodes 

%  from  the  description  of  the  voronoi  edges,  find  and  number  the  nodes 
X— ========== 

7. 

function  nodes  =  f ind_nodes(vx,vy) ; 

vxx  =  [vx ( 1 , : )  vx(2, :)] ;  vyy  =  [vy(l,:)  vy(2,:)]; 

[vyy_sort , i_sort]  =  sort (vyy); 

tmpy  =  vyy_sort(l);  tmpx  =  vxx(i_sort (1)) ;  ind  =  1;  num  -  1; 

7.  changed  Aug27,2002  so  as  not  to  eliminate  nodes  based  only  on  y-values  7. 
for  i=2 : length (vyy) , 
if  vyy_sort(i)  >  tmpy, 
ind  =  [ind  i]  ; 
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tmpy  =  vyy_sort(i); 
tmpx  =  vxx(i_sort(i) ) ; 
else 

eqFlag  =  0; 

for  j=l : length (tmpx) , 

if  vxx(i_sort(i))  ==  tmpx(j), 
eqFlag  =  1; 
end 
end 

if  eqFlag  ==  0, 
ind  =  [ind  i] ; 
tmpy  =  vyy_sort(i); 
tmpx  =  [tmpx  vxx(i_sort(i))] ; 
end 
end 
end 

nodes  =  [1 : 1 :length(ind) ;  ... 

vxx(i_ sort (ind)) ;  . . . 
vyy(i_sort(ind))] ; 

x 

X  get_closest 

X  calculate  the  closest  three  voronoi  nodes  to  the  specified  point 
X===s«====s=-================-*=============:===s=====;“=====-======: 

X 

function  ind  =  get_closest (nodes, pt) ; 

ni  =  nodes(l,:);  nx  =  nodes(2,:);  ny  =  nodes(3,:);  px  =  pt(l);  py 
=  pt  (2) ; 


delx  *  nx  -  px;  dely  =  ny  -  py;  dist.sq  =  delx.~2  +  dely.*2; 
[dist_sort,i-Sort]  -  sort (dist_sq) ; 

ind  =  i_sort(l:3); 

X 

X 

X  calc_threat_cost 

X  given  the  voronoi  edges  and  the  threat  locations  calculate 
X  the  cost  of  traveling  each  edge 

%  cost  =  l/d~4  *  edge  length  where  d  is  the  distance  from  the  threat 

•/,  to  the  midpoint  of  the  edge 

*/#  1/6  point  and  5/6  point  also  included 

X 

X  last  6  rows  of  threat_cost  correspond  to  costs  from  start  and  target 
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X  to  three  verticies  of  Voronoi  diagram 

\ 

function  threat_cost  = 

calc.threat .cost ( vx , vy , tx , ty , nodes , spt , sn.i , tpt , tn.i) ; 

bx  =  vx ( 1 , : )  +  (vx(2, :)-vx(l, :))/6;  by  =  vy(l,:)  + 

(vy(2, : )-vy(l, : ))/6;  mx  =  (vx(l , : )+vx(2, : ) )/2;  my  = 

(vy(l, :)+vy(2, :))/2;  ex  =  vx(l,:)  +  5*(vx(2, : )-vx(l , : ) )/6;  ey  = 
vy(l, : )  +  5*(vy(2, :)-vy(l, :))/6; 

L  =  sqrt((vx(l,:)-vx(2,:)).~2+(vy(l,:)-vy(2,:)).~2); 

[m,n]  =  size(vx);  p  =  length (tx) ; 

for  i=l:n, 
for  j=i:p, 

threat _b(i,j)  =  l/(.l*((bx(i)-tx(j))'2+(by(i)-ty(j))‘2)‘2)*L(i) ; 
threat_m(i, j)  =  l/(.l*((mx(i)-tx(j))‘2+(my(i)-ty(j))'2)‘2)*L(i) ; 
threat_e(i, j)  =  l/(.l*((ex(i)-tx(j))"2+(ey(i)-ty(j))~2)“2)*L(i) ; 

end; 

end; 

threat.mat  —  (threat .b  +  threat .m  +  threat.e)/3; 

bxs  -  spt(l)  +  (nodes (2, sn.i) -spt (l))/6;  bys  =  spt (2)  + 

(nodes (3 , sn_i) -spt (2) ) /6 ;  bxt  =  tpt(l)  +  (nodes(2,tn_i)-tpt(l))/6; 
byt  =  tpt (2)  +  (nodes (3, tn.i) -tpt (2) )/6;  mxs  = 

(spt ( 1) +nodes (2 , sn.i) ) /2 ;  mys  =  (spt(2)+nodes(3,sn.i))/2;  mxt  = 

(tpt ( 1) +nodes (2 , tn.i) ) /2 ;  myt  =  (tpt (2)+nodes(3,tn.i))/2;  exs  = 
spt  ( 1 )  +  5*  (nodes  (2 ,  sn.i) -spt  ( 1)  ) /6 ;  eys  =  spt  (2)  + 

5* (nodes (3 , sn.i) -spt (2) ) /6 ;  ext  «  tpt(l)  + 

5* (nodes (2, tn.i) -tpt (l))/6;  eyt  =  tpt(2)  + 

5* (nodes (3 , tn.i) -tpt (2) ) /6 ; 

Ls  =  sqrt ((spt (1) -nodes (2, sn.i)) . ~2+(spt (2)-nodes(3, sn.i)) . "2) ;  Lt 
= • sqrt ( (tpt (1) -nodes (2 , tn.i) ) . ~2+ (tpt (2) -nodes (3 , tn.i) ) . ~2) ;  for 
i=l :3, 

for  j“l:p, 

threat.b.st (i , j )  =  i/(. l*((bxs(i)-tx(j))~2+(bys(i)-ty(j))~2)~2)*Ls(i) 
threat_b.tg(i, j )  =  1/ ( . 1* ( (bxt (i) -tx ( j ) ) "2+ (byt (i) -ty ( j ) ) ~2) ~2) *Lt  (i) 
threat.m.st (i , j )  =  l/( . l*((mxs(i)-tx(j))~2+(mys(i)-ty(j)) ~2) ~2)*Ls(i) 
thr eat.m.tg ( i , j )  =  l/( . l*((mxt(i)-tx(j))~2+(myt(i)-ty(j))~2)~2)*Lt(i) 
threat.e.st(i, j)  =  l/(.l*((exs(i)-tx(j))~2+(eys(i)-ty(j))~2)~2)*Ls(i) 
threat. e.tg(i ,  j )  =  1/ ( .  l*((ext(i)-tx(j))~2-Keyt(i)-ty(j))~2)  2)*Lt(i) 
end; 
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end; 


threat .mat. start  =  (threat.b.st  +  threat _m_st  +  threat.e_st)/3; 
threat .mat .target  =  (threat .b.tg  +  threat .rn.tg  +  threat.e_tg)/3; 

threat.cost  =  [sum (threat _mat ,2) 9  sum (threat .mat .start ,2) ’ 
sum (threat .mat .target ,2) *]  ; 

% 

./============================.========.============================== 

%  gen. co st .mat 

*/,  generate  the  cost  adjacency  matrix  for  the  Voronoi  diagram 

./============_=====================:========.=:==============-====- 

7. 

function  A  =  gen.cost.mat (vx,vy, nodes, t cost ,spt,sni,tpt ,tni,Ct) ; 
Cf  =  1-Ct; 

ny  =  nodes(3,:);  7.  y  coordinates  of  nodes 

n  =  length(ny); 

vy.csrt  =  sort(vy,l);  7.  sort  y  vertices  by  column 

[dum,m]  =  size (vy.csrt) ; 

tcostn  *  tcost(i:m);  tcosts  =  tcost(m+l:m+3) ;  tcostt  = 
tcost(m+4:m+6) ; 

Af  =  inf  *  ones  (n+2)  ;  7.  create  initial  cost  adjacency  matrix 

At=Af ; 

7.At  =  inf*ones(n+2) ; 
magf  =  0;  magt  =  0; 

for  j  =  l:n, 

Af  Cj  » j)  =  0; 

At(j,j)  =  °» 
for  i  =  l:m, 

if  abs(vy.csrt(l,i)-ny(j))  <  eps, 
for  k  =  l:n, 

if  abs (vy.csrt (2, i) -ny (k) )  <  eps, 

Af (j+1 ,k+l)  =  norm([vx(l,i)-vx(2,i);vy(l,i)-vy(2,i)]); 
At(j+l,k+l)  =  tcostn(i) ; 
magf  =  magf  +  Af(j+l,k+l); 
magt  «  magt  +  At(j+l,k+l); 
end 
end 
end 
end 
end 
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Af (n+l,n+l)  =  0;  Af(n+2,n+2)  =  0;  At(n+l,n+l)  =  0;  At(n+2,a+2)  = 

0; 

*/,  Add  the  start  points  and  end  points  into  the  cost  matrix 
for  i=l:3, 

Af (1 ,sni(i)+l)  =  norm( [spt (l)-nodes(2,sni(i)) ;spt(2)-nodes(3,sni(i))]) ; 
Af (tni(i)+l ,n+2)  =  norm( [tpt (l)-nodes(2,tni(i) ) ;tpt(2)-nodes(3,tni(i))] ) 
magf  =  Af  (1 , sni(i)+l)  +  Af (tni(i)+l,n+2) ; 

At (1 ,sni(i)+l)  =  tcosts(i) ; 

At (tni (i)+l ,n+2)  =  tcostt(i); 
magt  =  At (l,sni(i)+l)  +  At(tni(i)+l,n+2) ; 
end 

Aff  =  Cf  *Af /magf ;  Att  =  Ct*At/magt;  A  =  Cf*Af/magf  +  Ct*At/magt; 

X  _ _ 

*/,  fixPaths 

X  remove  short  path  segments  and  adjust  velocity 

\ 

function  [vp_new,vel_new]  =  f ixPath(vp,vel) ; 

EPS  =  50; 

*/,  compute  current  transition  time 
L  =  0; 

for  j=l:size(vp,2)-l, 

L  =  L  +  norm(vp( [2:3] , j+l)-vp( [2;3] , j) ) ; 
end 

T  =  L/vel ; 

y#  remove  close  nodes 

vp2  =  vp(: ,1) ; 

for  j  =  1 :size(vp,2)-2, 

if  norm (vp2 ( [2:3] , end) -vp( [2:3] ,j+l))  >  EPS, 
vp2  =  [vp2,vp(: , j+1)] ; 
else 

vp2(:,end)  =  (vp2( : ,end)+vp( : , j+l))/2; 
end 
end 

if  norm(vp2( [2:3] ,end)-vp( [2:3] ,end))  >  EPS 
vp2  =  [vp2, vp( : ,end)] ; 
else 

vp2( : ,end)  =  vp(:,end); 
end 
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7*  remove  angles  close  to  180  degrees 
vp_new  =  vp2( : ,1) ; 
for  j  =  l:size(vp2,2)-2, 
wiml  =  vp_new( [2:3] ,end) ; 
wi  =  vp2(  [2:3] , j+1) ; 
wipl  =  vp2( [2:3] , j+2) ; 
ql  =  (wi-wiml)/norm(wi-wiml) ; 

q2  =  (wipl-wi)/norm(wipl~wi) ; 

beta  =  acos(ql,*q2) ; 
if  abs(beta)  >  pi/16, 

vp.new  =  [vp.new, vp2(: , j+1)] ; 
end 
end 

vp.new  =  [ vp.new, vp2(  :  ,end)]  ; 

7,  compute  new  velocity  to  match  transition  time 
L  =  0; 

for  j=l: size (vp.new, 2) -1, 

L  =  L  +  norm (vp_new(  [2:3] ,  j+l)-vp_new( [2:3] ,j) ) ; 
end 

vel.new  =  L/T;mex  -c  -DF0R_ WINDOWS  trajGen.cpp 
mex  -c  -DFOR.WINDOWS  rungkut.cpp  mex  -DFOR.WINDOWS  traj .states . cpp 
trajGen.obj  rungkut.obj  function  auto  -  my_autopilot.design(uO, 
thO,  C,  Ts); 

7*  u0  =  nominal  airspeed 
7.  thO  =  nominal  roll  angle 
%  C  =  airplane  coefficients 

%  General  constants 

g  =9.8;  7#  gravitational  constant  (m/s~2) 

S  =  C.bw*(C.cw+C.dw)/4;  7.  wing  size,  adjusted  for  taper  and  sweep  (nT2) 


q.dyn 

=  0.5*C.rho*u(T2; 

7*  dynamic  pressure 

b 

=  C.b; 

7.  wingspan  (m) 

cbar 

=  C. mean. chord; 

7.  mean  chord  (m) 

m 

=  C.m; 

7.  mass  (kg) 

lx 

-  C. J(l,i) ; 

7#  moment  of  inertia 

about  x-axis 

(kg-m) 

iy 

=  C. J(2,2) ; 

7.  moment  of  inertia 

about  y-axis 

(kg-m) 

Iz 

=  C. J(3,3) ; 

7.  moment  of  inertial 

.  about  z-axis 

(kg-m) 

7#  lateral  mode 


7#  lateral  directional  derivatives  (dimensional) 
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Y_beta 

Y-P 

Y_r 

Y_rudder 

L_beta 

L-P 

L_r 

L_aileron 

L_rudder 

N_beta 

N_p 

N.r 

N_aileron 

N.rudder 


q_dyn*S*C . C_Y_beta/ m ; 
q_dyn*S*b*C . C_Y_p/2/m/uO ; 
q_dyn*S*b*C .  C_Y_r/2/m/uO ; 
q_dyn*S*C . C_Y_rudder/m ; 
q_dyn*S*b*C . C_L_beta/Ix ; 
q_dyn*S*b~2*C . C_L__p/2/Ix/uO ; 
q_dyn*S*b~2*C . C_L_r/2/Ix/uO ; 
q_dyn*S*b*C . C_L_aileron/Ix ; 
q_dyn*S*b*C . C_L_rudder/Ix ; 
q_dyn*S*b*C . C_N_beta/Iz ; 
q_dyn*S*b~2*C . C  JLp/2/Iz/uO ; 
q_dyn*S*b"2*C . C_N_r/2/Iz/uO ; 
q_dyn*S*b*C .  CJLaileron/Iz ; 
q_dyn*S*b*C . C_N_rudder/Iz ; 


%  state  space  equations  for  lateral  mode 
AJLat  =  [. . . 

Y_beta/uO ,  Y_p/uO,  -(l~Y_r/uO),  g*cos(thO)/uO 
L_beta,  L_p,  L_r,  0;... 

N_beta,  N_p,  N_r,  0;... 

0,  1,  0,  0;... 

]; 

B.lat  =  [... 

0 ,  Y_rudder/uO ; . . . 

L_aileron,  Ljrudder; . . . 

N_aileron,  N_rudder; . . . 

0,  0;... 

]; 

C_lat  =  eye (4); 


switch  2, 

case  1,  7.  pole  placement  design 
a  -  7; 
b  *  14; 

poles  =  [-a-j*a,  -a+j*a,  -b-j*b,  -b+j*b] ; 
K_lat  =  place (A_lat ,B_lat, poles) ; 
case  2,  7,  LQR 

X  Q  =  diag( [10000,  0.01,  0.01,  100]); 

7,  Q  =  diag( [10000,  0.001,  0.001,  10]); 

Q  =  diag( [100 ,  0,  0.001,  100]); 

R  =  diag( [1,1]) ; 

K_lat  =  lqr (A_lat , B_lat , Q , R) ; 

L_lat  =  []  ; 
case  3,  7.  LQG  /  LTR 

Q  -  diag( [100,  0,  0.001,  100]); 

R  =  diag( [1,1]) ; 
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K_lat  =  lqr ( A_lat , B_lat , Q , R) ; 
r  =  100; 

Xi  =  diag([.001,  .001,  .001,  .001])  +  r*B_lat*B_lat’ ; 

Th  =  diag([.01,  .001,  .001,  .001]);  . 

L_lat  =  lqr(A_lat’ ,C_lat’ ,Xi,Th.) ’ ; 
eig(A_lat-B_lat*K_lat-L_lat*C_lat) 

[auto .A_lat_d, auto .B_lat_d]  =  c2d(A_lat-B_lat*K_lat-L_lat*C_lat ,  L_lat,Ts); 

end 


auto.A_lat  =  A_lat; 
auto.B_lat  =  B_lat; 
auto.K_lat  =  K_lat; 
auto.C_lat  =  C_lat; 
auto.L_lat  =  L_lat; 


•/•/•/•/ vvvvvvvvvvvvvvvvvvvvvvvv,/VVVVVVVVV//V®///#/8/,/V*/,/'/#/'/0/,/*/,Z#/V*/y////,/.y.V.#///////.'y 


*/,  longitudinal  mode 


*/#  longitudinal  directional  derivatives  (dimensional) 
%  X_u  =  0; 

X_u  =  q_dyn*S*C . C_X_0/m/ uO ; 

X.alpha  =  q_dyn*S*C.C_X_alpba/m; 

X_w  =  X_alpha/uO; 

X.elevator  =  q_dyn*S*C.C„X_elevator/m/uO; 

%  Z_u  =0; 

Z_u  =  q_dyn*S*C.C_Z_0/m/u0; 

Z_alpha  =  q_dyn*S*C.C_Z_alpha/m; 

Z_w  =  Z.alpha/uO ; 

Z_elevator  =  q_dyn*S*C.C_Z_elevator/m/uO; 

%  M_u  =  0; 

M_u  =  q_dyn*S*cbar*C.C_M_0/Iy/u0; 

M.alpha  =  q_dyn*S*cbar*C.C_M_alpha/Iy; 

M_w  =  M_alpha/uO ; 

M_q  =  q_dyn*S*cbar ~2*C . C_M_q/2/uO/Iy ; 

M_elevator  =  q_dyn*S*cbar*C.C_M_elevator/Iy; 

*/,  state  space  equations  for  lateral  mode 
A_lon  =  [. . . 

X_u,  X_w,  0,  -g*cos(th0) ;  . .  . 

Z_u,  Z_w,  uO,  -g*sin(thO) ; . . . 

M_u,  M_w,  M_q,  0; . . . 

0,  0,  1,  0;... 

]; 

B_lon  *  [. . . 

X_elevator; . . . 
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Z_elevator; . . . 

M_elevator; . . . 

0;... 

]; 

C_lon  =  eye (4); 
switch  2, 

case  1,  ‘/.  pole  placement  design 
a  =  7; 
b  =  14; 

poles  =  [-a-j*a,  -a+j*a,  -b-j*b,  -b+j*b] ; 

K_lon  =  place (AJLon,B_lon, poles) ; 

L_lon  =  []  ; 
case  2,  */.  LQR 

Q  =  diag([0. 1,0. 001, 0.001, 100]) ; 

R  =  1; 

K_lon  =  lqr (A_lon , B_lon ,  Q ,  R)  ; 

L_lon  =  []  ; 
case  3,  */•  LQG  /  LTR 

Q  *  diag([0. 1,0. 001, 0.001, 1000]); 

R  =  1; 

K_lon  =  lqr(A_lon,B_lon,Q,R) ; 
r  =  100; 

Xi  -  diag( [.001 ,  .001,  .001,  .001])  +  r*BJLon*BJLon, 
Th  =  diag([.01,  .001,  .001,  .001]); 

L_lon  =  lqrCA^lon’  ,C-lon)  ,Xi,Th)  9 ; 
eig(A_lon-B_lon*K_lon-L_lon*C_lon) 

[auto .A_lon_d, auto .B_lon_d]  =. . . 

c2d(A_lon-B_lon*K_lon-L_lon*C_lon,  L_lon,  Ts) ; 

end 

auto.A_lon  =  A_lon; 
auto.B_lon  -  B_lon; 
auto.C_lon  =  C_lon; 
auto.K_lon  =  K_lon; 
auto.L_lon  =  L_lon; 

'/,  parameter  file  for  simulations 

V. 

clear  all  startTime  =  0.0;  stopTime  =  3*60.0;  Ts  =  1/100; 
global  P;  global  uavdata;  global  env;  global  M; 

%  number  of  vehicles 

p  -  l; 
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’/*  initial  configuration  of  uavs 

'/,  fields  are:  uavdata(num) . ic  =  [XO;  YO;  psiO;  VO;  hO;  hdotO] 


where  (X0,  Y0) 

=  inertial  position  of  aircraft 

psiO 

=  heading  angle  in  inertial  coordinates 

VO 

=  speed 

hO 

=  altitude 

hdotO 

=  climb  rate 

•/fuavdata(l) . ic  =  [-25;  -5;  pi/4;  1*5;  1;  0]; 

uavdata(l) . ic  =  [-500;  -100;  pi/4;  11;  1;  0] ; 


uavdata(2) .ic  = 

C  25; 

-5; 

3*pi/4; 

l; 

l.i; 

0]; 

uavdata(3) . ic  = 

[  25; 

40; 

-3*pi/4; 

l; 

1.2; 

0]; 

uavdata(4) .ic  = 

[  -5; 

40; 

-3*pi/4; 

l; 

1.3; 

0]; 

uavdata(5) . ic  = 

[-10; 

-5; 

3*pi/4; 

l; 

1.4; 

l — I 

O 

%  target  locations 

env. targets ( : , : , 1)  =  20*[... 

20,  5;  . .  . 

10,  20;... 

-20,  25;... 

-10,  35;... 

-10,  0;... 

]; 

*/,  home  locations  (places  the  UAVs  fly  to  after  mission  is  completed 
env. home  =  20* [. . . 


100, 

0; 

100, 

10 

100, 

20 

100, 

30 

100, 

40 

]; 


'/,  initially  known  threat  locations 
env. threats  =  20*[... 


-1,  4;.. 

12,  9;.. 

-12,  8;.. 
-18,  11;.. 
8,  14;.. 

18,  16;.. 
-8,  17;.. 

-11,  17;.. 

-20,  18;.. 
15,  20;.. 

2,  23;.. 
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-12,  23;... 

6,  27;... 

13,  27;... 

-1,  28;... 

19,  29;... 

-13,  30;... 

12,  33;... 

-2,  35;... 

0,  12;... 

5,  12;... 

-7,  19;... 

5,  19;... 

7,  35;... 

9,  2;... 

0,  -5;...  7.  new  threats 

17,  -3;... 

25,  5;... 

28,  12;... 

25,  25;... 

15,  44;... 

4,  47;... 

-15,  40;... 

-22,  32;... 

-28,  12;... 

-17,  -8 

]; 

7,  pop  up  threats,  only  become  visible  when  airplane  is 
7,  within  a  threshhold  of  the  threat 
env. popup  =  20*  [••• 

-15,  -2;... 

-8,  S;... 

7.  22,  20;... 

]; 


7.  airplane  coefficients  and  control  gains 
for  i=l:P, 

7,uavdata(i)  .psidot_max  =  0.2375;  7.  works! 

uavdata(i) .psidot.max  =  0.271;  %  turning  rate  limit 

uavdata(i)  .v_max  =  11.5;  7.  11;  7.  maximum  plane  velocity 

uavdata(i)  .v_min  =9.4;  7.  10;  7.  minimum  plane  velocity 

y.uavdata(i)  ,tau_psi  =  0.55;  %works! 

uavdata(i)  . tau_psi  =  0.55;  %3 . 81 ;  ’/»  autopilot  constant 

uavdata(i)  .tau_V  =  0.192;  7.0.14;  7.  autopilot  constant 

uavdata(i)  .tau.ha  =  3.5118;  7.  2.426  7.  autopilot  constant 
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uavdat a  ( i ) .  t  au_hb  =  0 . 8447 ;  */• 

uavdata(i) . size  =  0.08;  */• 

uavdata(i) .k_hdot  =10;  */• 

uavdata(i) .k_h  =  10;  */* 

uavdata(i) .k_x  -  1;  /• 

uavdat  a  (i )  .k_y  =1;  7. 

uavdata(i) . k_z  =  1;  7» 

uavdata(i) . turn_param  =1;  /• 

% 

uavdata(i)  .equivdist  =1;  7* 

7. 

uavdata(i) .kappa  =  .01;  7* 

*/• 

uavdata(i) .K1  =  10*eye(2);  % 

uavdata(i)  .K2  -  l*eye(2);  7* 

uavdata(i)  .gam  =  1/10;  7* 

uavdata(i) .bkstepK  =  10.0; 

uavdat a (i ) .bkstepG  =  1.0; 


X  tracker  parameters 
uavdat a (i) .track_gamO  =  0.5; 
uavdata(i) .track _ gam 1  =  0.5; 
uavdata(i) . track_gam2  =  0.5; 
uavdata(i) .track_kl  =  2.0; 
uavdata(i) . track„eps_vmin  =  0.4; 
uavdata(i) . track_eps_vmax  =  1.5; 
7*uavdata(i)  ,track_eps_wmax  =  0.05;  7«works! 
uavdata(i) . track_eps_wmax  =  0.1; 
end 


%  number  of  storage  elements  for  path 
M  =  100; 


7*  calculate  initial  conditions 
sim_ic  =  calcInitialCond; 


dof!2paramf unction  [sys,x0,str,ts]  =  plotEnv(t ,x,u, 
'/.plotTraj ectory  -  plot  trajectory  being  planned. 


7. 

7. 

% 

% 

7, 

7. 


Expected  inputs  are: 

u(l) 

-  trajectories 

u(2) 

-  waypoint  paths 

u(3) 

-  UAV  states 

u(4) 

-  target  locations 

u(5) 

-  threat  locations 

autopilot  constant 
scale  on  airplane  drawing 
control  gain 
control  gain 
control  gain 
control  gain 
control  gain 
l=parameterized  turn  in 
trajectory  generator 
l=trajectories  have  same 
length  as  Voronoi  path 
parameter  for  trajectory 
generator 

control  gain  for  ATT 
control  gain  for  ATT 
adaptation  gain  for  ATT 


flag , plane , env , M , P) 
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% 

•/,  modified  8/3/01  -  RWB 

%  9/26/02  -  RWB 


switch  flag, 


xxxxxxxxxxxxxxxxxx 

X  Initialization  X 

xxxxxxxxxxxxxxxxxx 

case  0, 

[sys,xO,str,ts]  = 


mdlInitializeSizes(P,M) ; 


xxxxxxxxxx 

X  Update  x 
XXXXXXXXXX 

case  2, 

sys  =  mdlUpdat e(t,x,u, plane, env, P, M) ; 

XXXXXXXXXX 

%  Output  X 

xxxxxxxxxx 

case  3, 
sys  =  []  ; 

xxxxxxxxxxxxx 

X  Terminate  7* 

XXXXXXXXXXXXX 

case  9, 

sys  =  [] ;  X  do  nothing 


xxxxxxxxxxxxxxxxxxxx 

#/,  Unexpected  flags  % 

XXXXXXXXXXXXXXXXXXXX 

otherwise 

error ( [ 1 unhandled  flag 


end 

*/,end  dsfunc 


’  ,num2str (f lag)]  ) ; 


X 

*/*  mdllnitializeSizes 

%  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-f  unction. 

X 


function  [sys,xO,str,ts]  =  mdllnitializeSizes (P,M) 
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sizes  =  simsizes;  sizes .NumCont States  =  0;  sizes .NumDiscStates  = 
3+P*(l+l+l+l+l+3*M) ; 

%  x(l)  =  state  to  indicate  plot  initialization 

%  x(2)  -  num^threats.stored 

7,  x(3:3+P-l)  =  figure  handle  for  paths 

7,  x(3+P:3+2*P-l)  =  figure  handle  for  trajectories 

7,  x(3+2*P:3+3*P-l)  =  figure  handle  for  UAVs 

7,  x(3+3*P :3+4*P-l)  =  figure  handle  for  targets 

%  x(3+4*P)  =  figure  handle  for  threats 

%  x(4+4*P:4+4*P-l)  =  num_waypoints_ stored 

%  x(4+4*P+l :4+4*P+P*3*M)  =  stored  waypoint  paths 

sizes.NumOutputs  =  0;  sizes .Numlnputs  -  -1; 

sizes . DirFeedthrough  =  1;  sizes .NumSampleTimes  =  1; 


sys  «  simsizes (sizes) ; 

xO  =  zeros (sizes. NumDiscStates, 1) ;  str  =  []  ;  ts  =  [00]; 

7.ts  =  [.05  0]; 

7.ts  =  [.2  0]; 

7.  end  mdllnitializeSizes 

7. 

7.  mdlUpdate 

7,  Handle  discrete  state  updates,  sample  time  hits,  and  major  time  step 
7*  requirements. 

7,—^“====““===“=====“====“=========“==“======“===““===““== 

7. 

function  xup  =  mdlUpdate (t ,x,u, plane, env,P,M) 

initialize  =  x(l); 

fig.threats  =  x(2); 

fig_path  =  x(3:3+P-l); 

f ig_tra j  =  x(3+P:3+2*P-l); 

fig_uav  =  x(3+2*P : 3+3*P~l) ; 

fig_target  =  x(3+3*P:3+4*P-l) ; 

num_threats_ stored  =  x(3+4*P) ; 

for  i=l:P, 

num_waypoints_stored(i)  =  x(4+4*P+(3*M+l)*(i-l)) ; 

path_stored( : , i)  =  x(5+4*P+(3*M+l)*(i-l) : . . . 

4+4*P+ (3*M+1) *i-l) ; 

end 

for  i=l:P, 

traj ( : , i)  =  u(l+3* (i-1) : 3+3* (i-1) )  ; 
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end 

for  i-l:P, 

num_waypoints(i)  =  u(l+3*P+(3*M+2)*(i-l)) ; 

vel(i)  =  n(2+3*P+(3*M+2)*(i-l)) ; 

path( : , i)  =  u(3+3*P+(3*M+2)*(i-l) : . . . 

3+3*M-l+3*P+(3*M+2)*(i-l) ) ; 

end 

for  i  =  1:P, 

x_plane(:,i)  =  u(l+P*(5+3*M)+6*(i-l) : . . . 

6+P* (5+3*M)+6* (i-1) ) ; 

end 

for  i=l:P, 

target (:,i)  =  u(l+2*(i-l)+P*(ll+3*M) : . . . 

2+2*(i-l)+P*(ll+3*M)) ; 

end 

num_threats  =  u(l+P*(13+3*M)) ; 

=  u(2+P*(13+3*M) : . . . 
l+2*mim_threats+P* (13+3*M) ) ; 

=  reshape (tmp,num_threats, 2) ; 


tmp 

threats 


if  initialize==0, 

%  initialize  the  plot 
initialize  =  0.05; 
figure(l),  elf 
axis (20* [-10 , 50 , -30 , 30] ) ; 
grid  on 

xlabel ( * Y  (East)’); 
ylabel ( ' X  (North)' ); 
hold  on 


%  plot  threats 

fig_threats=plot (threats (: ,2) ,threats(: ,1) , ’ . * . ’EraseMode’ , ’none’) 
zoom  on 

%  plot  way-point  path 
for  i=l:P, 

tmp  =  reshape (path(l : 3*num_waypoints (i) ,i) ,3,  ... 

num_ waypo int  s ( i ) ) ; 

f ig_path(i)  =  plot(tmp(2, :) ,tmp(l, :) , ’green’ , ’EraseMode’ , ’none’) 
end 

•/,  plot  trajectory 
for  i=l:P, 

fig_traj(i)  =  trajPlotlnit (traj (1:2, i) » 'red' , 'xor') ; 
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end 


•/,  plot  UAVs 
for  i=l:P, 

f ig_uav(i)  =  planePlotlnit (x.plane (1 : 3 , i) , ’blue ’ .plane . size , ’xor ’ ) ; 
end 

7,  plot  targets 
for  i=l:P, 

f ig_target (i)  =  plot (target (2, i) , target (1 ,i) ,* s’ ) ; 
end 

else  7,  do  this  at  every  time  step 

7*  make  sure  we  only  do  this  every  20th  of  a  second 
if(t  >=  initialize), 

initialize  =  t  +  0.05; 

7.  plot  threats 
7#f  igure  (3) 

if  num_threats~=num_threats_stored, 

set(fig_threats, ,XData> ,threats( : ,2) , ' YData* , threats (:, 1) ) ; 
drawnow 

end 

%  plot  way-point  path 
for  i=i:P, 

if  norm(path(: , i) -path_stored( : ,i))~=0, 

tmp  =  reshape (path( 1 : 3*mim_waypoints (i) ,i) ,3,num_waypoints(i)) ; 

set (f ig_path(i) , 'XData* ,tmp(2, :) , 'YData’ ,tmp(l, :)) ; 

drawnow 

end 

end 

7.  plot  trajectory 
for  i=i :P, 

trajPlot(f ig.traj (i) ,traj (1 : 2 , i) ) ; 
end 

%  plot  airplanes 
for  i=l:P 

planePlot (f ig_uav(i) ,x_plane(l :3,i) , 'blue’ , plane. size) ; 

end 

7.  plot  targets 
for  i-l:P, 

set (f ig_target (i) , 'XData' , target (2, i) , 'YData1 , target (1 ,i)) ; 

end 
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end; 

end 

xup  -  [initialize;  fig.threats;  fig.path;  f ig^traj ; . . . 

fig.uav;  fig_target;  num_threats_stored] ; 
for  i=l :P» 

xup  =  [xup;num_:waypoints_stored(i) ;  . .  . 
reshape (path_stored ( : ,i) ,3*M,1)] ; 

end 

*/,end  mdlUpdate 


X - 

% - - - 

•/,  User  defined  functions 

X - 

X - 


function  handle=planePlotInit(y,  color  ,APSIZE,  mode)  ; 

%  planePlotlnit:  plot  plane  at  configuration  y  with  color 
ap  =  apTranslate(apRotate(20*APSIZE*apData,y(3) ) ,y (1 :2)) ; 
handle=plot (ap(2, :) ,ap(l, :) , color, ’EraseMode* ,mode) ; 


\UUUUU  /•/.  7.  /•  /.  /.  U  /.  /.  /.  /.  /.  /.  /•  /.  /•  /.  /•  /•  /•  /•  /•  /•  /•  /•  /•  I* !'  '•  '• 


function  planePlot (handle ,y, color ,APSIZE) ; 

*/,  planePlot:  plot  plane  at  the  configuration  given  by  y. 
ap  =  apTranslate (apRotate (20*APSIZE*apData,y (3) ) ,y(l :2)) ; 
set (handle , 1 XDataJ , ap(2 , : ) , 9 YData* , ap(l , : ) ) ; 


drawnow 


function  handle=trajPlotInit(y, color ,mode) ; 

%  trajPlotlnit :  plot  desired  trajectory  position 
th  =  0:2*pi/10:2*pi; 

rab  =  20*5*  [y(l)+cos(th) ;  y(2)+sin(th)] ; 
handle=plot (rab (2 , : ) ,rab(l , : ) , color , ’EraseMode* ,mode) ; 


xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 


function  trajPlot (handle, y) ; 

%  trajPlot:  plot  desired  trajectory  position 
th  =  0:2*pi/10:2*pi; 

R  =  20* ( . 5) ; 

rab  =  [y(l)+R*cos (th) ;  y(2)+R*sin(th)] ; 

set (handle, ’XData* ,rab(2, :) , ,YData> ,rab(l, :)) ; 


drawnow 
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xmmxxmmmxxm 

function  apOut  =  apRotate(ap,phi) ; 

%  apRotate:  rotate  an  airplane  defined  by  ap  by  the  angle  phi 
R  =  [cos(phi)  -sin(phi);  sin(phi)  cos(phi)]; 
apOut  =  R*ap; 


xxxxxxxxxxxxxxxxxxxxxxx 

function  apOut  =  apTranslate(ap,r) ; 

X  apTranslate:  translate  the  airplane  by  the  vector  r 
apOut  =  ap  +  r*ones(l ,length(ap)) ; 


xxxxxxxxxxxxxxxxxxxxxxx 


function  ap  =  apData; 

X  apData:  define  the  points  on  the  aircraft 
ap  =  [ . . . 

5,0;... 

-5,-5;... 

-2,0;... 

-5,5;... 

5,0;... 


function  [sys,x0,str ,ts]  = 
t argetMgr (t , x , u , flag , uavdat a , env ,M,P, xxO) 
X  Target  Manager 

X 

X  Modified  3/26/02  -  RB 


switch  flag, 


xxxxxxxxxxxxxxxxxx 

X  Initialization  X 

XXXXXXXXXXXXXXXXXX 

case  0, 

[sys,xO,str,ts]  = 


mdllnitializeSizes (uavdata, env , M , P ,xxO) ; 


xxxxxxxxxx 

X  Update  x 
XXXXXXXXXX 

case  2, 

sys  =  mdlUpdate(t,x,u,uavdata,env,M,P) ; 


XXXXXXXXXX 
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X  Output  % 

xxxxxxxxxx 

case  3, 

sys  -  mdlOutputs(t ,x,u,uavdata,env,M,P) ; 


xxxxxxxxxxxxx 

'/,  Terminate  */, 

mmmxm 

case  9, 

sys  *  []  ;  %  do  nothing 


xxxxxxxxxxxxxxxxxxxx 

X  Unexpected  flags  */, 

xxxxxxxxxxxxxxxxxxxx 

otherwise 

error ( [ *  unhandled  flag  - 


end 

•/.end  dsfunc 


* ,num2str(f lag)]  ) ; 


X  _ ___ _ 

X  mdllnitializeSizes 

%  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-f unction. 


A 

function  [sys,xO,str,ts]  =  mdllnitializeSizes (uavdata,env,M,P,xxO) 

•/.input  ( ’  In  targetmgr  init  * )  ; 

sizes  *  simsizes; 

sizes. NumContStates  =  0; 

sizes .NumDiscStates  =  P  +  2*P  +  P; 

%  for  i=l :P, 

•/,  state  (i)  =  x(i); 

%  target (:,i)  =  x(l+2*(i-l)+P:2+2*(i-l)+P) ; 

*/,  target_ptr(i)  =  x(i+3*P) ; 

X  end 

sizes. NumOutputs  =  2*P; 

X  y  =  targets 

sizes.Numlnputs  =  P; 

X  for  i=l : P, 

X  new_path_flag(i)  =  u(i) ; 

X  end 

sizes .DirFeedthrough  =  1; 
sizes .NumSampleTimes  =  1; 
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sys  =  simsizes (sizes) ; 

%  initialize  state 

xO  =  xxO;  7*  done  in  calcInitialCond 


str  =  []  ; 
ts  =  [0  0]; 

7,  end  mdllnitializeSizes 

7.  _  _ 

7,  mdlUpdate 

7,  Handle  discrete  state  updates,  sample  time  hits,  and  major  time  step 
7*  requirements. 


function  xup  =  mdlUpdate(t ,x,u,uavdata,env,M,P) 


7.input  ( J  In  targetmgr  update * ) ; 
for  i=l:P, 

state(i)  =x(i); 

target ( : ,i)  =  x(l+2*(i-l)+P:2+2*(i-l)+P) ; 

target_ptr (i)  -  x(i+3*P) ; 

new_target_flag(i)  =  u(i) ; 

end 

for  i=l:P, 

switch  state (i), 

case  1,  7*  loop  here  until  a  path  is  requested 
if  new_target_flag(i)==l , 
state(i)  =  2; 
else 

stated)  =  1; 
end 

case  2,  7*  output  the  next  target 
if  target_ptr(i)+l  >  size(env.targets( : , : ,i) ,1) , 
target _ptr(i)  =  1; 
else 

target_ptr (i)  =  target_ptr(i)+l ; 
end 

target (:,i)  =  env. targets (target.ptr (i) , : ,i) * ; 
stated)  -  3; 

case  3,  7.  wait  for  confirmation  of  received  target 
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if  new_target_flag(i)==0, 
stated)  =  1; 
else 

state (i)  =  3; 
end 

otherwise 

dispd Unknown  state1) 
end 
end 

xup  =  [ . . . 

state; . . . 

reshape (target ( : , i) , 2*P , 1) ; .  . . 
target_ptr; . . . 


'/♦end  mdlUpdate 

7. 

y===============================================================™ 

7,  mdlOutputs 

'/♦  Return  Return  the  output  vector  for  the  S-f unction 
./_====_===============:=====_=:===================:=================^= 

7. 

function  y  =  mdlOutputs (t ,x,u,uavdata,env,M,P) 

'/.input d In  targetmgr  output1); 
for  i=l:P, 

state(i)  =  x(i) ; 

target ( : ,i)  =  x(l+2*(i-l)+P:2+2*(i-l)+P) ; 

target _ptr(i)  =  x(i+3*P); 

new_target_flag(i)  =  u(i) ; 

end 

7.  output  the  paths  for  each  UAV 
y  =  reshape (target ,2*P, 1) ; 

'/.end  mdlUpdate 

function  [sys,xO,str ,ts]  =  att (t ,x,u,flag,uavdata,P) 

7.  s-f unction  implementation  of  UAV  dynamics  (assuming  an  autopilot) . 

7. 

X  Modified:  9/26/02  -  RWB 

7. 


switch  flag, 
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xxxxxxxxxxxxxxxxxx 

X  Initialization  X 

xxxxxxxxxxxxxxxxxx 

case  0, 

[sys ,x0 , str , ts] =mdllnit ializeSizes (uavdata , P) ; 


xxxxxxxxxxxxxxx 


X  Derivatives  X 

XXXXXXXXXXXXXXX 


case  1, 

sys=mdlDer ivat ives (t , x , u , uavdat  a ,  P ) ; 


xxxxxxxxxxx 

X  Outputs  X 

xxxxxxxxxxx 

case  3, 

sys=mdlOutputs(t ,x,u, uavdata, P) ; 


xxxxxxxxxxxxxxxxxxx 

X  Unhandled  flags  X 

XXXXXXXXXXXXXXXXXXX 

case  {  2,  4,  9  >, 
sys  =  []  ; 


xxxxxxxxxxxxxxxxxxxx 

X  Unexpected  flags  X 
XXXXXXXXXXXXXXXXXXXX 


otherwise 

error ([* Unhandled  flag  =  } ,num2str(f lag)]  ) ; 


end 

X  end  csfunc 


X 

— — — — - - 

X  mdllnitializeSizes 

X  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-f unction. 


function  [sys , xO , str , ts] ^mdllnitializeSizes (uavdata , P) 


sizes  =  simsizes;  sizes .NumCont States  =  6*P;  sizes .NumDiscStates 
=  0;  sizes. NumOutputs  =  6*P;  sizes .Numlnputs  =  3*P; 

sizes .DirFeedthrough  =  0;  sizes .NumSampleTimes  =  1; 
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sys  =  simsizes (sizes) ; 

•/.uavdata.ic:  x,  y,  psi,  v,  h,  hdot 

7,x0  =  uavdata(l)  . ic+[1.5;  1;  0.8;  0;  0;  0]; 

xO  =  uavdata(l) . ic;  Xadding  some  initial  tracking  errors 
for  p=2 : P , 

xO  =  [xO;  uavdata(p) .ic] ; 
end  str  =  []  ;  ts  =[0  0]; 


7,  end  mdllnitializeSizes 

% 

7.  mdlDerivatives 

7.  Return  the  derivatives  for  the  continuous  states. 

% 

function  xdot=mdlDer ivat ives (t , x , u , uavdat a , P ) 


7.  interpret  inputs 
for  p=l:P, 

X(p)  =  x(l+6*(p-l)) ; 
Y (p)  =  x(2+6*(p-l) ) ; 
psi(p)  =  x(3+6*(p-l)) ; 
v(p)  =  x(4+6* (p-1) ) ; 
h(p)  =  x (5+6* (p-1) ) ; 
hdot(p)  =  x(6+6*(p-l)) ; 
psi_c(p)  =  u(l+3*(p-l)) ; 
v_c(p)  =  u(2+3*(p-l)) ; 
h_c(p)  =  u(3+3*(p-l)) ; 


end 


7*  uav  dynamics 
for  p=l:P, 

xdot (l+6*(p-l) )  =  v(p) *cos (psi (p) ) ; 
xdot (2+6* (p"l) )  =  v(p)*sin(psi(p)); 

xdot (3+6* (p-1))  =  uavdata(p) .tau_psi*(psi^c(p)-psi(p)) ; 


7.  saturate  velocity 

vdot^temp  =  uavdata(p) .tau_V*(v_c(p)-v(p)) ; 

7,if  (v(p)  >=  uavdata(p)  . v_max)&(vdot_temp>0)  , 

7.  xdot  (4+6*  (p-1))  =  0; 

7,elseif  (v(p)  <=  uavdat  a  (p)  .  v_min)&(vdot  _temp<0)  , 
7,  xdot  (4+6*  (p-1))  =  0; 

7*else 
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xdot (4+6* (p-1))  =  vdot_temp; 

*/,end 

xdot (5+6* (p-1))  =  hdot(p); 

xdot (6+6* (p-1))  =  -uavdata(p)  .tau_hb*hdot(p)  +  uavdata(p)  .tau_ha*(li_c(p)-li(p)) 

end 

7  end  mdlDerivatives 

7  _ _ 

7  mdlOutputs 

7  Return  the  block  outputs. 

7 

function  y=mdlOutput s (t , x , u , uavdat a , P) 


7,  output  the  states 
y  =  x; 


7.  end  mdlOutputs 


function  y  =  uav_linear_f orces(u,C) 

7*  uavforces 

7. 

7*  forces  acting  in  the  MAGICC  lab  UAV 

7. 

7,  Modification  History: 

%  1/11/03  -  RWB 

7. 


7  inputs 

7  z 

=  [u(l);  u(2)]; 

7  h 

=  u(3); 

V 

= 

u(4) 

alpha 

= 

u(5) 

beta 

= 

u(6) 

phi 

= 

u(7) 

theta 

= 

u(8) 

psi 

= 

u(9) 

P 

= 

u(10) ; 

q 

= 

u(ll); 

r 

= 

u(12); 

delta.er 

= 

u(13) ; 

delta.el 

u(14); 

7  inertial  position  of  UAV 
7*  altitude 
7  air  speed 
7  angle  of  attack 
7  sideslip  angle 
7  roll 
7  pitch 
7  yaw 

7  roll  rate 
7  pitch  rate 
7  yaw  rate 

7  deflection  of  right  el e von 
7  deflection  of  left  elevon 


delta_rr  =  u(15);  7  deflection  of  right  rudder 

delta_rl  =  u(16);  7  deflection  of  left  rudder 
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delta_t 


u(17); 


*/,  engine  input 


rudder  =  (delta_rr+delta„rl)/2; 
elevator  -  (delta_er+delta_el)/2; 
aileron  =  (delta_er-delta_el)/2; 
throttle  =  delta_t; 

X  forces  due  to  gravity 
F.gravity  =  [ . . . 

-C.m*C.g*sin (theta) ;  . .  . 
C.m*C.g*cos(theta)*sin(phi) ; . . . 
C.m*C.g*cos(theta)*cos(phi)] ; 

cbar  =  C.mean^chord; 
b  =  C. wingspan; 

•/,  longitudinal  Force  and  Moment  coefficients 
C_X  =  C.CJC_0... 

+  C.C_X_alpha  *  alpha... 

+  C . C_X_q  *  (cbar/V)  *  q. . . 

+  C.C_X_elevator  *  elevator... 

+  C.C_X_throttle  *  throttle; 

C_Z  -  C.C_Z_0. . . 

+  C . C_Z_alpha  *  alpha . . . 

+  C.C_Z_q  *  (cbar/V)  *  q. . . 

+  C.C_Z_elevator  *  elevator... 

+  C.C_Z_throttle  *  throttle; 

C_M  -  C.C_M_0. . . 

+  C.C_M_ alpha  *  alpha... 

+  C . C_M_q  *  (cbar/V)  *  q... 

+  C.C_M_elevator  *  elevator... 

+  C.C_M_throttle  *  throttle; 

%  lateral  Force  and  Moment  coefficients 
C_Y  «  C . C_Y_0 . . . 

+  C.C_Y_beta  *  beta... 

+  C . C_Y_p  *  (b/2/V)  *  p. . . 

+  C . C_Y_r  *  (b/2/V)  *  r. .. 

+  C.CLY_aileron  *  aileron... 

+  C.C_Y_rudder  *  rudder; 

C_L  -  C.C_L_0 . . . 

+  C.C_L_beta  *  beta... 

+  C . C_L_p  *  (b/2/V)  *  p... 

+  C.C„L_r  *  (b/2/V)  *  r. . . 

+  C.C_L_aileron  *  aileron...  * 

+  C.C_L_rudder  *  rudder; 
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C_N  =  C .  C__N_0 . . . 

+  C.C_N_beta  *  beta... 

+  C . C_N_p  *  (b/2/V)  *  p... 

+  C.CJ.r  *  (b/2/V)  *  r.  . . 

+  C.C_N_aileron  *  aileron. . . 
+  C.C_N_rudder  *  rudder; 


%  wing  size,  adjusted  for  taper  and  sweep 
S  =  C.bw*(C.cw+C.dw)/4; 

*/,  dynamic  pressure 
q_dyn  =  0 . 5*C . rho*V~2 ; 

%  aerodynamic  forces  and  moments 
F_aero  =  q_dyn  *  S  *  [  C_X;  C_Y;  C_Z] ; 

Tlaero  =  q.dyn  *  S  *  [  (b/2)  *  C_L;  cbar  *  C_M;  (b/2)  *  C_N] ; 


y,  output  total  forces  and  torques 
F  =  F_gravity  +  F_aero; 

T  =  T_aero; 

7,  create  output 

y  =  [F;  T] ; 

function  xdot  =  uavdynamics(u,C) 

'/,  uavdynamics 

7. 

%  specify  dynamics  of  the  MAGICC  lab  UAV 
7. 

7.  Modification  History 
7.  12/6/02  -  RWB 

7. 


m  =  C.m; 

J  =  C.J; 

X  =  u(l) 

%  inertial  X  coordinate 

(north) 

Y 

=  u(2) 

•/,  inertial  Y  coordinate 

(East) 

h 

=  u(3) 

%  altitude  in  inertial 

coordinates 

V 

=  u(4) 

*/,  Airspeed 

alpha  =  u(5) 

*/#  angle  of  attack 

beta 

=  u(6) 

%  sideslip  angle 

phi 

=  u(7) 

•/,  roll  angle 

theta  =  u(8) 

%  pitch  angle 

psi 

=  u(9) 

%  yaw  angle 
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p  =  u(10) ;  X  roll  rate 

q  =  u(ll);  %  pitch  rate 

r  *  u(12);  X  yaw  rate 

F  =  u(13: 15)  ;  X  forces  in  body  frame 

T  =  u(16:18);  X  torques  in  body  frame 

•/,  rotation  matrices 
R_roll  «  [... 

1,  0,  0;... 

0,  cos(phi),  sin(phi);... 

0,  -sin (phi ) ,  cos(phi)]; 

R.pitch  =  [. . . 

cos (theta) ,  0,  sin (theta) ; . . . 

0,  1,  0;... 

-sin (theta) ,  0,  cos (theta)] ; 

R_yaw  =  [ . . . 

cos(psi) ,  sin(psi) ,  0;... 

-sin(psi),  cos(psi),  0;... 

0,  0,  1]; 

X  compute  body  axis  velocities 

u  =  V*cos  (alpha) *cos  (beta)  ;  7*  body  velocity  along  x  axis 
v  =  V*sin(beta) ;  X  body  velocity  along  y  axis 

w  *  V*sin(alpha)*cos(beta) ;  X  body  velocity  along  z  axis 

X  compute  xdot 

X_Y_h_dot  =  R.yaw’+R.pitch’+R.roll’+Cu;  v;  -w]  ; 

Vdot  =  [cos (alpha) *cos (beta) ;  sin(beta) ;  sin(alpha)*cos(beta)] **F/m; 

alphadot  =  ( 1/V/cos (beta) )* (1/m* (-F(l)*sin (alpha) +F (3) *cos (alpha))) . . . 

+q  ~(p*cos(alpha)+r*sin(alpha))*tan(beta)  ; 

betadot  =  (l/V/m)*(-F(l)*cos (alpha) *sin(beta)  +  F(2)*cos(beta) . . . 

-  F(3)*sin(alpha)*sin(beta))  +  p*sin(alpha)  -  r*cos(alpha) ; 
Edot  =  inv([l,  0,  -sin(theta) ; . . . 

0,  cos(phi),  sin(phi) *cos (theta) . 

0,  -sin(phi) ,  cos (phi) *cos (theta)] )* [p;q;r] ; 

Wdot  =  inv(J) * (-cross ( [p;q;r] , J* [p;q;r] )  +  T) ; 

xdot  =  [X_Y_h_dot ;  Vdot;  alphadot;  betadot;  Edot;  Wdot]; 

function  [sys,x0,str ,ts]  =  wayPointMng(t ,x,u,flag,plane,M,env,xxO) 
7.WayPointMng 

X  Way  point  manager  -  feeds  the  trajectory  generator  N  way  points 
*/,  at  a  time. 

7* 

X  Modified  3/26/02  -  RB 
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N=3 ;  X  number  of  way  points  passed  to  trajectory  generator 


switch  flag. 


xxxxxxxxxxxxxxxxxx 

X  Initialization  X 

% iXXXXXXXXXXXXXXXXX 

case  0, 

[sys,x0,str ,ts]  = 


mdllnit ializeSizes (N , M , plane , env , xxO) ; 


xxxxxxxxxx 

7.  Update  X 

xxxxxxxxxx 

case  2, 

sys  =  mdlUpdate(t, x,u,N,M, plane, env) ; 


xxxxxxxxxx 

7,  Output  7# 

xxxxxxxxxx 

case  3, 

sys  =  mdlOutputs(t ,x,u,N, M, plane, env) ; 


xxxxxxxxxxxxx 

7.  Terminate  % 

xxxxxxxxxxxxx 

case  9, 

sys  =  []  ;  7*  do  nothing 


xxxxxxxxxxxxxxxxxxxx 

%  Unexpected  flags  X 

xxxxxxxxxxxxxxxxxxxx 


otherwise 

error C [ 1 unhandled  flag  =  * ,num2str (f lag)] ) ; 

end 

Xend  dsfunc 


X 

./i============================^===============================s============== 

X  mdllnitializeSizes 

X  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-f unction. 

X 


function  [sys,x0,str ,ts]  =  mdllnitializeSizes (N,M, plane , env, xxO) 
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'/input  ( *  In  waypointmng  init*); 

sizes  =  simsizes; 

sizes. NumContStates  =  0; 

sizes .NumDis cStates  =  l+l+3+l+6*M+l+l+l+3*M+l; 

X  state  =  x(l); 

X  request_new_path  =  x(2); 

X  wO  =  x(3:5) ; 

X  wpPtr  =  x(5); 

X  wpQueue  =  x(6:5+6*M); 

X  num_threats_prev  =  x(6+6*M); 

%  counter  =  x(7+6*M); 

X  pathJLen  *  =  x(8+6*M); 

sizes .NumOut puts  -  1+3+1+9; 

X  y CD  =  request .new.path; 

X  y(2)  =  wO; 

X  y(3)  *  new_vel; 

X  y(4)  =  wpQueue (1:9) 

sizes.Numlnputs  =  -1; 

X  new_path_len  =  u(l); 

X  new.vel  -  u(2) ; 

X  new_path  =  u(3:2+3*M); 

X  new_wp_f lag  =  u(3+3*M) ; 

X  traj  *  u(4+3*M:6+3*M) ; 

X  numjthreats  =  u(7+3*M) ; 

X  threats  =  reshape(u(8+3*M:7+3*M+2*num_threats) ,num_ threats, 2) ; 

sizes. DirFeedthrough  =1; 
sizes .NumSampleTimes  =  1; 

sys  =  simsizes (sizes) ; 

xO  =  xxO; 

str  =  []  ; 
ts  =  [0  0]; 

X  end  mdllnitializeSizes 

%  _ _ 

X  mdlUpdate 

X  Handle  discrete  state  updates,  sample  time  hits,  and  major  time  step 
X  requirements . 

X 

function  xup  =  mdlUpdate (t ,x,u,N, M, plane, env) 
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•/.input ('In  waypointmng  update'); 

state  ®  x(l) ; 

request _new_path  =  x(2); 

wO  =  x(3:5) ; 

wpPtr  =  x(6) ; 

wpQueue  =  x(7:6+6*M); 

num_threats_prev  =  x(7+6*M) ; 

counter  =  x(8+6*M); 

path_len  =  x(9+6*M); 

stored_path  =  x(10+6*M:9+9*M) ; 

stored_path_len  =  x(10+9*M); 

path  =  wpQueue  ( 1 :  3*path_.len)  ; 

new_path_len  =  u(l); 

new_vel  -  u(2) ; 

new^path  =  u(3:2+3*new_path_len) ; 

new_wp_f lag  =  u(3+3*M) ; 

traj  =  u(4+3*M: 6+3*M) ; 

num^threats  =  u(7+3*M) ; 

threats  =  reshape (u(8+3*M:7+3*M+2*num_thr eats) ,num_threats,2) ; 

y,*/.  input  (' in  way  point  manager  update'); 

*/,if  t  >-  46.50, 

X  t 

X  state 
%  wpPtr 

%  wpQueue (wpPtr : wpPtr  +  11) 

%  path_len 

X  input ( ' top  of  update ' ) ; 

y,end 

switch  state, 

case  1,  ’/.state  1  loops  here  until  pop-threat  OR  changed  path  OR  request  flag  from  traj. 
if  num_threats“=num_threats_prev, 
state  =  6; 

elseif  (new_path_len~=stored_path_len)  I  ... 
(norm(stored_path(l:3*stored_path_len)-new_path)~=0) , 

state  =5; 

elseif  new_wp_f lag==l , 
state  =  2; 
else 

state  =  1; 
end 

case  2,  ’/.got  a  request  flag  from  traj.gen 
•/.check  for  pop-up  threats 
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7, try  to  send  next  wp 

7, if  3  or  less  wps  are  in  the  Q,  go  to  state  4 
7, otherwise  go  to  state  3 
wpPtr  =  wpPtr+3;  ; 


if  numjthreats~=num_.threats_prev, 
state  =  6; 
elseif  (path_len- (wpPtr- l)/3)  <=  3, 
state  =  4 
else 

state  =  3 
end 


case  3,  ‘/.state  3  loops  until  pop-threat  OR  request  flag  from  traj.gen  has  gone  down, 
if  num_threats~=:num_threats_prev) 
state  =  6; 

elseif  new_wp_f lag==0 , 
state  =  1; 
else 

state  =  3; 
end 


case  4,  7.state  4  is  when  there  are  not  enough  wps  in  Q 
request _new_path  =  1; 

wO  =  wpQueue (3*path_len-2 : 3*path_len) ;  %set  wO  to  last  wp  in  Q 

if  num_threats~=num_threats_prev,  7*check  for  a  pop-up  threat 
state  =  6; 
else 

state  -  5;  7.otherwise  go  to  state  5 

end 

case  5,  7*wait  here  for  the  path  to  change  because  we  ran  out  of  wps  in  our  Q 
if  numjthreats~=num.  threats  prey.  7*check  for  a  pop-up  threat 
state  =  6; 

elseif  (new_path_len~=stored_path_len)  I  ...  %  check  if  the  path  has  chang* 

(norm(stored_path(l :3*stored_path_len) -new_path)  =0) , 

re que s t _ne w_pat h  —  0;  7*the  path  has  changed  so  we 

wpQueue(l: (3*(path_len-l)-wpPtr+l))  =  wpQueue (wpPtr :3*(path_len-l)) ; 
wpQueue (3* (path_len-l) -wpPtr+2 : 3* (path.len-1)  -wpPtr+3*new_path_len+l) . . . 

=  new_path(l :3*new_path_len) ; 
path_len  =  new_path_len+((path_len-l)  -  (wpPtr-l)/3) ; 
wpPtr  «  1; 

stored.path  =  [new_path(l : 3*new_path_len) ; . . . 

zeros (3*M-3*new_pathJLen, 1)] ; 
stored_path_len  =  new_path_len; 


82 


if  path^len  <=  3,  '/.ran  out  of  wps  again 
state  =  4; 
else 

state  =  1; 
end 
else 

state  =  5; 
end 

case  6,  %pop-up  threat 
request_new_path  =  2; 
num_threats_prev  =  num_ threats; 
wO  =  traj; 
state  =  7; 

case  7,  */*pop-up  threat  cont  -  loop  here  until  path  changes 
if  (new_path_len~=stored_path_len)  I  ... 
(norm(stored_path(l : 3*stored_path_len) -new_path)  =0) , 
request _new_path  =  0; 

wpQueue(l:3*new_path_len)  =  new_path(l :3*new_path_len) ; 
path_len  =  new_path_len ; 
wpPtr  =  1; 

stored_path  =  [new_path(l : 3*new_path_len) ; . . . 

zeros (3*M-3*new_path_len,l)] ; 
stored_path_len  =  new_path_len ; 

if  stored_path_len  <=  3, 
state  =  4; 
else 

state  =  1; 
end 
else 

state  -  7 ; 
end 

otherwise 

disp(' Unknown  state') 
end 

xup  =  [. . . 
state; . . . 

request_new_path; . . . 
wO ;  .  .  . 
wpPtr ; . . . 
wpQueue ; . . . 
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num_threats_prev; . . . 
counter; . . . 
path_len; ... 
storecLpath; . . . 
stored_path_len; . . . 

]; 

Xend  mdlUpdate 

X  _ 

X  mdlOutputs 

X  Return  Return  the  output  vector  for  the  S-f unction 

X 

function  y  =  mdlOutputs (t,x,u,N,M, plane, env) 

X input ( *  In  waypointmng  output*); 

state  =  x(l) ; 

request _new_path  =  x(2) ; 

wO  =  x(3:5) ; 

wpPtr  =  x(6) ; 

wpQueue  =  x(7:6+6*M); 

num_threats_prev  =  x(7+6*M); 

counter  =  x(8+6*M); 

path_len  =  x(9+6*M) ; 

path  =  wpQueue (1 : 3*path_len) ; 

new_path_len  =  u(l); 

newjvel  =  u(2) ; 

new_path  =  u(3:2+3*new_path_len) ; 

new_vp_f lag  =  u(3+3*M) ; 

traj  =  u(4+3*M : 6+3*M) ; 

num_ threats  =  u(7+3*M) ; 

threats  =  reshape (u(8+3*M \ 7+3*Mt2*num__threats) ,num_ threats, 2) ; 

X input ( *  in  way  point  manager  output*); 

y  =  [request_new_path;  wO;  new_vel;  wpQueue (wpPtr :wpPtr+8)3 ; 

Xend  mdlUpdate 

function  [sys,xO,str ,ts]  =  wpp(t ,x,u, flag, uavdata, env, M,P,xxO) 

XWaypoint  Path  Planner 

X 

X  Modified  3/26/02  -  RB 
switch  flag, 
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X  Initialization  X 

xxxxxxxxxxxxxxxxxx 

case  0, 

[sys,xO,str,ts]  = 


mdllnit ializeSizes (uavdata , env , M , P , xxO) ; 


xxxxxxxxxx 

X  Update  X 

XXXXXXXXXX 

case  2, 

sys  =  mdlUpdate(t ,x,u, uavdata, env, M,P) ; 


XXXXXXXXXX 

x  output  % 
xxxxxxxxxx 

case  3, 

sys  =  mdlOutputs (t , x , u , uavdata , env , M , P) ; 

xxxxxxxxxxxxx 

X  Terminate  X 

XXXXXXXXXXXXX 

case  9, 

sys  *  [] ;  X  do  nothing 


xxxxxxxxxxxxxxxxxxxx 

X  Unexpected  flags  X 

XXXXXXXXXXXXXXXXXXXX 


otherwise 

error ( [ * unhandled  flag  = 

end 

Xend  dsfunc 


* ,num2str (flag)]  ) ; 


X  mdllnitializeSizes 

X  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-f unction. 

X===========^=======:==================^================================== 


X 

function  [sys,xO,str,ts]  =  mdllnit ializeSizes (uavdata, env, M,P,xxO) 


Xinput ( , In  wpp  init  * ) ; 
sizes  =  simsizes; 
sizes .NumCont States  =  0; 

sizes . NumDis cStates  =  1  +  P  +  (2+3*M)*P  +  2*P  +  P; 
X  num_threats_prev  =  x(l); 

X  for  i=l:P, 
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X  stated)  =  x(i+l) ; 

%  path.( : ,i)  =  x(l+(2+3*M)*(i-l)+P: (2+3*M)*i-l+P) ; 

X  path(:,i)  =  [num^waypoints;  velocity;  waypoints  padded  with 
X  zeros]  ; 

X  target_stored(: ,i)  = 

*/#  x(l+2*(i_l)+P*(3+3*M)  :2+2*(i~l)+P*(3+3*M)) ; 

7.  new„target_f lag(i)  =  x(i+l+P+(2+3*M)*P+2*P) 

X  end 

sizes.NumOutputs  =  P  +  (2+3*M)*P; 

X  y(l:P)  =  flag  requesting  new  target 

X  y (P+1  rend)  =  paths 

X  path(:,i)  =  [num_ waypoints;  velocity;  waypoints  padded  with 
X  zeros] ; 

sizes. Numlnputs  =  2*P  +  4*P  +  1  +  2*size (env. threats, 1) .. . 

+  2*size (env. popup, 1) ; 

X  for  i=l:P, 

X  target (:,i)  =  u(l+2*(i-l)  :  2+2* (i-1)); 

X  new_path_f lag(i)  =  u(l+4*(i-l)+2*P) ; 

X  traj (: ,i)  =  u(2+4*(i-l)+2*P:4+4*(i-l)+2*P) ; 

X  end 

X  num_ threats  =  u(l+3*P); 

X  threats  =  reshape (u(2+6*P: l+2*num_threats+6*P) ,  ... 

X  num_threats,2) ; 

sizes. DirFeedthrough  =  1; 
sizes. NumSampleTimes  =  1; 

sys  =  simsizes (sizes) ; 

X  initialize  state 
xO  =  xxO; 

str  *  []  ; 
ts  =  [0  0]; 

X  end  mdllnitializeSizes 

X 

X  mdlUpdate 

X  Handle  discrete  state  updates,  sample  time  hits,  and  major  time  step 
X  requirements. 

y.===============s= - - - - 

X 

function  xup  =  mdlUpdate (t ,x,u,uavdata,env,M,P) 

Xinput ( 9  In  wpp  update , ) ; 
num_threats_prev  =  x(l); 
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for  i=l :P, 
stated) 
path(: ,i) 

target_stored( : ,i) 
new_target_flag(i) 
target (: ,i) 
new_path_f lag(i) 
traj  (:  ,i) 
end 

num_threats 

threats 


x(i+l)  ; 

x(l+(2+3*M)*(i-l)+P+l : (2+3*M)*i+P+l) ; 

x(l+2* (i-l)+P* (3+3*M) +1 : 2+2* (i-1) +P* (3+3*M)+l) ; 

x (i+l+P+ (2+3*M) *P+2*P) ; 

u(l+2*(i-i)  :  2+2*d-D); 

ud+4*d-l)+2*P) ; 

u(2+4* (i~l) +2*P : 4+4*  (i~i)  +2*P) ; 

u(i+6*P) ; 

reshape  (u(2+6*P :  l+2*num_threats+6*P)  ,  ... 


num_ threats ,  2) ; 


for  i=i:P, 


switch  state (i). 


case  1,  */,  check  for  new  path  request 
if  new_path_flag(i)==l, 
state (i)  =  2; 

elseif  new_path_flagd)==2, 
state (i)  =  4; 
else 

state(i)  =  1; 
end 


case  2,  */,  simple  target  request 
new_target_flag(i)  =1; 
stated)  =  3; 

case  3,  %  make  sure  target  changes 

if  norm(target(: ,i)”target_stored(: ,i ))~=0, 
state(i)  =  4; 
new_target__flag(i)=0; 
else 

stated)  =  3; 
end 

case  4,  %We  have  a  new  path  -  send  to  traj Gen 
vel  «  (uavdatad)  .v_max  +  uavdatad)  . v_min)/2;  %  pick  velocity  inbetween  min  and  max 
[vp,  vel]  =  generateVPpath (threats ,  traj(:,i)»  target (:,i)»  vel); 
path_len  =  size(vp,2); 
path( : , i)  =  [path_len; . . . 
vel ; . . . 

reshape ( [vp (2 : 3 , : ) ;  uavdata(i) . ic(5)*ones(l ,path_len)] , . . . 

3*path_len,  1);... 
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zeros(3*(M-path_len) ,1)] ; 
stated)  =  5; 

case  5,  %  wait  for  acknowledgment  from  trajGen 
if  new_path_f 1 ag ( i ) ==0 , 
state(i)=l; 
else 

state(i)=5; 

end 

otherwise 

disp(’ Unknown  stated 
end 
end 

xup  =  [.  . . 

num.threats; . . . 
state;... 

reshape (path, (2+3*M)*P, 1) ; . .  ■ 
reshape (target (: ,i) ,2*P,1) ; . . . 
new_target__flag; . . . 

]; 


7,end  mdlUpdate 
•/. 

•^=====3=======:==========:==:======— ==========— —==:=—::==:==: 

%  mdlOutputs 

%  Return  Return  the  output  vector  for  the  S-f unction 


function  y  =  mdlOutputs (t ,x,u,uavdata,env,M,P) 


#/,input(,In  wpp  output’); 
for  i-l:P, 

path(: ,i)  *  x(l+(2+3*M)*(i-l)+P+l : (2+3*M)*i+P+l) ; 

new_target_f lag(i)  =  x(i+l+P+(2+3*M)*P+2*P) ; 
end 


*/t  output  the  paths  for  each  UAV 
y  =  [new_target_f lag;  reshape (path, (2+3*M)*P, 1)] ; 

'/.end  mdlUpdate 
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10.2  Supporting  C  files 

_ */  /*  Name:  rungkut . c 

*/  /*  Created:  8/1/2002  */  /* 

*/  /*  Does  a  4th  order  Runge-Kutta  */  /*  approximation 

to  the  continuous  part  of  */  /*  g_uav  -  i.e.  x,  y,  and  psi 
*/  /* - */ 

#include  "trajGen.h" 

extern  uavlnfo  g.uav  [MAXPLANES] ;  /**<  global  variable 

holding  all  uav  info  -  declared  in  trajGen.c  */  extern  uavlnfo 
g_uav_prev [MAXPLANES] ;  /**<  global  variable  holding  the  previous 

uav  info  for  g_uav  -  declared  in  trajGen.c  */ 

/**  Self  contained  function  to  run  5th  order  Runge-Kutta-Fehlberg 
*/  void  rungkutfehl (double ,  double,  int,  int,  double,  double); 

/**  global  function  pointer  used  to  point  to  yprime  to  be  used  in 
Runge-Kutta  algos  */  double  (★g^yprimeptr)  (double,  double); 

/**  function  that  specifies  yprime  and  is  pointed  to  by 
g_yprimeptr  */  double  yprime (double ,  double); 

/*  int  main(){ 

int  N  =  10; 
int  i  =  0; 
double  t  =  0; 
double  w  =  1; 
g_yprimeptr  =  yprime; 
for(;i<N;i++){ 

printf  07.-1 .  Ilf \t%-l .  lOlf  \n’‘  ,t , w) ; 
v  =  rungkut4(0. l,t ,w) ; 
t  +=  .1; 

> 

printf  ( ”7.-1 .  Ilf  \t7.-l .  lOlf  \n”  ,  t ,  w) ; 

rungkutfehl (0. 1,0. 1,0,1, l,pow(10.0, -5.0)) ; 

}  */ 

void  uavCopy(int  id,  uavlnfo*  K)  //  copies  g_uav_prev  to  K.  This 
so  iterations  in  the  algorithm  {  //  don’t 

mess  up  g_uav_prev.  We  use  the  previous  g_uav  information 

int  i;  //to  match  Matlab  sequence  and  to  avoid  making  a  dependency  on 
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//  order  of  calls  to  rungkut  and  stateUpdate 

K->x  =  g__uav_prev  [id]  .x; 

K~>y  =  g_uav_prev [id] .y; 

K->z  =  g_uav_prev[id]  .z; 

K->psi  =  g_uav_prev [id] .psi; 

K->Vel  =  g_uav_prev [id] .Vel; 

K->turnFlag  -  g_uav_prev[id] .turnFlag; 

K->state  «  g_uav_prev [id]  .state; 

K->newSigmaFlag  =  g_uav_prev[id] .newSigmaFlag; 


for(i=0; i<N; i++) 

K->currentSigma[i] .x  «  g«uav_prev [id] . currentSigma[i] .x; 
K-> current Si gma[i] .y  =  gjuav.prev [id]  . currentSigma [i] . y ; 
K->currentSigma[i] .z  *  gjuav_prev [id] . currentSigma[i] .z; 

> 


K->startNormal . x 
K->startNormal . y 
K->startNormal . z 
K->endNormal .x  = 
K->endNormal.y  = 
K-> endNormal. z  = 


=  g_uav_prev [id] . startNormal . x ; 
*  g juav„prev [id] . startNormal . y ; 
=  g_uav_prev [id] . startNormal . z ; 
g__uav_prev  [id]  .  endNormal .  x ; 
g_uav_pr ev  [id]  .  endNormal .  y ; 
g_uav_prev  [id]  .  endNormal .  z ; 


//  does  4th  order  Runge-Kutta  approximation 
//  CHANGE:  the  z  value  (height)  is  approximated  the  same 
//  way  as  x,  y,  and  psi,  but  ’trajCenDerivative*  currently 
//  does  not  return  the  derivative  of  z,  so  the  approximation 
//  is  not  reliable  with  respect  to  z 

void  rungkut Tr a j Gen (int  id,  double  vel,  planelnfo*  Param,  double 
time_step){ 

uavlnfo  Kl; 
uavlnfo  K2; 
uavlnfo  K3; 
uavlnfo  K4; 

uavCopy ( id ,  &K 1 ) ; 
uavCopy(id,  &K2) ; 
uavCopy (id,  &K3) ; 
uavCopy (id,  &K4)  ; 

//  now  K1-K4  all  hold  the  same  information  as  g_uav_prev 
traj GenDer ivat ive (&K1 ,  vel,  Param);  //  find  1st  approximation 
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Kl.x  *=  time.step; 

Kl.y  *=  time.step; 

Kl.z  *-  time^step; 

Kl.psi  *=  time^step; 

//  use  K1  to  modify  K2  for  setup  to  2nd  approximation 
K2.x  +=  Kl.x/2; 

K2.y  +=  Kl.y/2; 

K2.Z  +=  Kl.z/2; 

K2.psi  +=  Kl.psi/2; 

trajGenDerivative(&K2,  vel,  Param) ;  //  find  2nd  approximation 

K2.x  *=  time_step; 

K2.y  *=  time_step; 

K2.z  *=  time_step; 

K2.psi  *=  time_step; 

//  use  K2  to  modify  K3  for  setup  to  3rd  approximation 
K3.x  +=  K2.x/2; 

K3.y  +=  K2.y/2; 

K3.z  +=  K2.z/2; 

K3.psi  +=  K2.psi/2; 

trajGenDerivative (&K3 ,  vel,  Param);  //  find  3rd  approximation 
K3.x  *=  time_step; 

K3.y  *=  time.step; 

K3.z  *=  time_step; 

K3.psi  *=  time.step; 

//  use  K3  to  modify  K4  for  setup  to  4th  approximation 
K4.x  +=  K3.x; 

K4.y  +=  K3.y; 

K4.z  +=  K3.z; 

K4.psi  +=  K3.psi; 

trajGenDerivative(&K4,  vel,  Param);  //  find  4th  approximation 
K4.x  *=  time.step; 

K4.y  *=  time_step; 

K4.z  *=  time_step; 

K4.psi  *=  time.step; 

//  all  approximations  are  done  -  use  to  find  the  hew  g_uav 
g_uav[id].x  +=  (Kl.x  +  2*K2.x  +  2*K3.x  +  K4.x)/6.0; 
g_uav[id].y  +=  (Kl.y  +  2*K2.y  +  2*K3.y  +  K4.y)/6.0; 

/ /g.uav . z  +=  (Kl.z  +  2*K2.z  +  2*K3.z  +  K4.z)/6.0; 
g_uav  [id]  .  z  =  1 ; 

g.uav [id] .psi  +=  (Kl.psi  +  2*K2.psi  +  2*K3.psi  +  K4.psi)/6.0; 
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//  Approximation  the  finds  best  time  step  -  however,  since  our  time  step 
//  is  fixed,  this  cannot  be  used 

void  rungkutfehl (double  hmax,  double  hmin,  int  a,  int  b,  double 
alpha,  double  tol){ 

int  flag  =  1; 
double  R; 
double  delta; 
double  h  =  hmax; 
double  w  =  alpha; 
double  t  =  a; 

printf  (M,/,l  .81f \t#/*l . 81f \ttol  =  %lf\nH  ,t  ,w,tol)  ; 
while (f lag) { 

double  K1  =  h*g_yprimeptr (w,t) ; 

double  K2  =  h*g_yprimeptr(w  +  Kl/4,t+h/4); 

double  K3  =  h*g_yprimeptr(w  +  9*K2/32  +  3*Kl/32,t+3*h/8) ; 

double  K4  =  h*g_yprimeptr(w  +  7296+K3/2197  +  -7200*K2/2197  +  1932*Kl/2197,t  +  12*h/13) ; 
double  K5  =  h*g_yprimeptr(w  +  -845+K4/4104  +  3680+K3/513  +  -8*K2  +  439*Kl/216,t  +  h) ; 
double  K6  =  h*g_yprimeptr(w  +  -ll*K5/40  +  1859*K4/4104  +  -3544*K3/2565  +  2*K2  +  -8*Kl/2 

R  =  fabs(Kl/360  -  128*K3/4275  -  2197*K4/75240  +  K5/50  +  2*K6/55)/h; 

delta  =  . 84*pow ( (t ol/R) , 0 . 25) ; 

if(R<=tol){ 
t  =  t  +  h; 

w  =  w  +  25*K1/216  +  1408*K3/2565  +  2197*K4/4104  -  K5/5; 
printf  ("%1 . 71f  \t7,i . 71f  \t%l . 71f  \nH , t , w,h)  ; 

> 

if (delta<=. 1){ 
h  =  . l*h; 

> 

else  if (delta>=4){ 
h  =  4*h; 

> 

else{ 

h  =  delta*h; 

> 

if(h  >  hmax) 
h  =  hmax; 


92 


if(t>=  b) 
flag  =  0; 

else  if(t  +  h  >  b) 
h  =  b-t; 

else  if (h  <  hmin){ 
flag  =  0; 

printf ("minimum  h  exceeded******\n") ; 
return ; 

} 

} 

} 


double  yprime (double  y,  double  t){ 


return  (  y  ); 

y  _ */  /*  Name:  trajGen.c 

*/  /*  Created:  8/5/2002  */ 

_ */  /**  This  file 

contains  an  implementation  for  the  S-function 
*  used  in  the  trajectory  generation  program 
*/ 


#include  "trajGen.h" 

//#def ine  UTURN_DEBUG  1 
//#define  DEBUG  1 

uavlnfo  g_uav [MAXP LANES] ;  /**<  global  variable  holding  all 

uav  info  */  uavlnfo  g_uav_prev [MAXPLANES] ;  /**<  global  variable 

holding  the  previous  uav  info  for  g_uav  */  int  PLANE.PTR  =0; 


void  uav_prev_Copy (int  id);  /**<  copies  g_uav  to  g_uav_prev 

to  unlink  stateUpdate  from  rungkut  */  int 
isLeftOf Line(vect3* , vect3* , double , double) ; 


//  performs  rungkut  and  state  update  for  trajectory  generation 
void  tra j GenUpdate ( int  id,  double  vel,  vect3  *sigma,  planelnfo* 
Param,  double  time_step)  { 

/*★  call  stateUpdate  to  update  the  state  machine  */ 
g_uav[id] .Vel  =  vel; 

stateUpdate (&g_uav [id] ,  vel,  sigma,  Param); 

/**  calculate  the  answer  to  the  derivates  -  sets  g_uav  */ 
rungkut Tr a j  Gen (id ,  vel, Param, time_step) ; 

/**  copy  g_uav  to  g_uav_prev  */ 
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uav_prev_Copy(id) ; 


return; 

} 

//  initializes  all  of  the  fields  in  g_uav 
int  trajGenlnitializeCplanelnf o*  Param,  double*  wpsO)  { 
if (PLANE.PTR  MAXPLANES) 

return  -1; 


g_uav [PLANE.PTR]  .x  = 
g_uav[PLANE_PTR] .y  = 
g.uav [PLANE_PTR] .z  = 
g„uav [PLANE_PTR] .psi  = 
g_uav [PLANE _PTR] .Vel  - 


Param->ic .xO; 
Param->ic.yO; 
Param->ic .zO; 
Param->ic .psiO; 
Param->ic .vO; 


g_uav [PLANE_PTR] .turnFlag  =  0; 
g_uav [PLANE_PTR] . state  =  0; 

g_uav [PLANE_PTR] . currentSigma [0] . x  =  wps0[0] 
gjuav [PLANE^PTR] . cur rent Sigma [0] .y  =  wpsO[l] 
g_uav [PLANE.PTR] . currentSigma [0] .z  =  wps0[2] 
g_uav [PLANE_PTR] .currentSigma [1] .x  =  wps0[3] 
g_uav [PLANE.PTR] .currentSigma [1] .y  =  wps0[4] 
g_uav [PLANE^PTR] . currentSigma [1] . z  =  wps0[5] 
g.uav [PLANE_PTR] .currentSigma [2]  .x  *  wps0[6] 
g_uav[PLANE_PTR] .currentSigma [2] .y  =  wps0[7] 
g_uav [PLANE_PTR] . currentSigma [2] .z  =  wps0[8] 

g.uav [PLANE_PTR] .newSigmaFlag  =  0; 
g_uav [PLANE_PTR] . st artNormal . x  =  0; 
g_uav [PLANE.PTR] . startNormal .y  »  0; 
g_uav[PLANE_PTR] . startNormal .z  =  0; 
g_uav [PLANE_PTR] . endNormal . x  =  0 ; 
g_uav [PLANE_PTR] . endNormal . y  =  0 ; 
g_uav [PLANE_PTR] . endNormal .  z  ~  0 ; 


uav_pre v_Copy (PLANE_PTR) ;  //  make  sure  g_uav_prev  contains  the  initial  values 


PLANE.PTR++; 

return  (PLANE_PTR  -  1) ; 

> 


//  performs  state  update  for  trajectory  generation 

void  stateUpdate (uavlnf o*  uav,  double  vel,  vect3  *wpN,  planelnfo* 

Param)  { 
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vect3  cl,cr,ql,q2,qTmp,c,v1n; 

double  vectorNorm,  a,a2,  b,b2,  d,  psidot,  normal.ang, beta, kappa  =  Param->kappa,  eps_d  -  0 
int  hor,hor2,turn,turn2,clin,s,next_state,p,acc; 

double  radius  =  vel/Param~>psidot_max ;  //  radius  of  the  2  side  circles  of  the  pi 

double  df  =  2*radius ;  //  distance  that  is  considered  far  from 

#ifdef  UTURN_DEBUG 

printf ("guav  state  is  */,d\n"  ,uav->state) ; 

#endif  #ifdef  DEBUG 

printf ("TRAJGEN:  — >  START  stateUpdate\n") ; 

#endif 

if C  (uav->newSigmaFlag  ==  0)  && 

(normN (wpN ,  uav->currentSigma)  >  0.001)  )  //  if  the  input  waypoints  differ  signific 

{  //  from  the  current  sigma,  then  we  are  r 

//We  have  a  new  set  of  waypoints  given  the  manager 

sigmaCopy(wpN,  uav-> current Sigma) ;  //  copy  the  input  waypoints  to  current  si 

uav->turnFlag  =  0; 
uav->state  =  0; 


#ifdef  DEBUG 

printf ( "TRAJGEN :  New  set  of  waypoints\nM) ; 

printf ("TRAJGEN:  — >  END  stateUpdate\n") ; 
#endif 


return; 

} 

//  find  center  of  circle  on  left  of  uav 
cl.x  =  uav->x  +  radius*sin(uav->psi) ; 
cl.y  «  uav->y  -  radius*cos(uav->psi) ; 
cl.z  =  uav->z; 

//  find  center  of  circle  on  right  of  uav 
cr.x  =  uav->x  -  radius* sin (uav->psi ) ; 
cr.y  *  uav->y  +  radius* cos (uav->psi) ; 
cr.z  =  uav->z; 


//  x  coordinate  of  center  of  circle  on  left 
//  y  coordinate  of  center  of  circle  on  left 

//  z  coordinate  of  center  of  circle  on  le 


//  x  coordinate  of  center  of  circle  on  right 
//  y  coordinate  of  center  of  circle  on  right 

//  z  coordinate  of  center  of  circle  on  ri 


//  ql  and  q2  are  unit  vectors  along  the  current  and  next  path  edges  respectively 
ql.x  =  uav->currentSigma[l] .x  -  uav->currentSigma[0] .x; 
ql.y  =  uav->currentSigma[l] .y  -  uav->currentSigma[0] .y ; 
makeUnitVector(feql) ; 


q2.x  =  uav->currentSigma[2] .x  -  uav->currentSigma[l] .x; 
q2.y  *  uav->currentSigma[2] .y  -  uav->currentSigma[i] .y ; 
makeUnitVector(&q2) ; 
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turn  *  turndir (ql , q2) ;  //  find  out  which  way  the 

// 

// 

// 

if (turn  !=  2  &&  fabs(acos(ql.x*q2.x  +  ql.y*q2.y)) 

#ifdef  DEBUG 

printf ("TRAJGEN:  Shallow  turn,  setting  turn 

#endif 

turn  =  0; 

} 

#ifdef  DEBUG 

printf ("Update  turn  is  y,d\n" ,turn) ; 

#endif 

//  find  the  line  equation  y=ax+b  for  the  line  from  sigmal  to  sigma2 
hor  -  getlinevals(uav->currentSigma[l]  ,uav->  cur  rent  Sigma  [2]  ,&a,&b); 
d  =  dist2path(uav->currentSigma[0]  ,ql,uav->x,uav->y) ;  1 1  how  far  off  ql  the  uav  is 

acc  =  12;  /**<  accuracy  that  we  know  the  equivalent  distance  kappa  to  is  2~(-acc)  * 

next_state  =  1;  /**<  default  next  state  */ 

switch  (uav->turnFlag)  //  where  we  are  at  in 

{  // 

// 

// 

// 

// 

// 

// 

// 

case  0:  //  track  a  line 

#ifdef  DEBUG 

printf ("TRAJGEN:  STATE  0\n") ; 

#endif 

switch(Param->turn_param)  //  how  we  execute  turns:  0,  QUESTION:  what  is  this? 

{  //  1,  parameterized  turn  in  trajectory 

case  0: 

#ifdef  DEBUG 

printf ("TRAJGEN:  Turn  param  is  set  to  0\n"); 

#endif 

//  figure  out  the  unit  vector  from  the  begining  waypoint  to  the  uav 
qTmp.x  =  uav“>x  -  uav->currentSigma[0] .x; 
qTmp.y  =  uav->y  -  uav->currentSigma[0] .y ; 
makeUnitVector(feqTmp) ; 


a  turn:  0,  track  a  line 

1,  finish  the  last  part  of  kappa  t 

2,  track  a  line  until  a  new  waypoi. 

3, 

4, 

5, 

6, 

7, 

8,  finish  the  last  part  of  kappa  t 


uav  should  turn  ->  0,  no  turn 

1,  counter-clockwise 
-1,  clockwise  (right- 

2,  u-turn 

<=  PI/10H 
to  0\n"); 
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turn2  «  turndir (qTmp , ql )  ;  //  find  which  direction  we  need  to  turn  to  get  back  on  the 
//  find  the  line  equation  ’y  =  a2x  +  b2>  for  the  line  from  sigmaO  to  sigmal 
hor2  =  getlinevals(uav->currentSigma[0]  ,uav->currentSigma[l] ,&a2,&b2); 

//  check  to  see  if  we  are  too  far  away  from  path 

d  =  dist2path(uav~>currentSigma[0]  ,ql,uav->x,uav->y) ; 

if(  d  >=  df  ) 

i 

#ifdef  DEBUG 

printf ("TRAJGEN:  Uav  is  far  off  the  path  with  turn  param  set  to  0\n"); 

#endif 

if  (  (clintersect (hor2,a2,b2, cl. x, cl. y, radius, uav->x,uav->y)  &&  (tum2  <  0))  II 
(clintersect (hor2 , a2 ,b2 , cr . x , cr . y , radius ,uav->x , uav->y)  &&  (turn2  >  0))  ) 
uav->turnFlag  =  5; 

else  $> 

uav->turnFlag  =  3; 

//  make  sure  that  our  normal  is  pointing  towards  the  path 

if(  sign( (uav->x-uav->currentSigma[0]  .x)*ql.y  -  (uav->y-uav->currentSigma[0]  .y) *q 
normal _ang  =  atan2(qi .x,-ql .y) ; 
else 

normal_ang  =  atan2(-ql ,x,ql .y) ; 
next_state  =  sign (sin (normal _ang  -  uav->psi)); 

} 

> 

if  (  (clintersect (hor , a , b , cl . x , cl . y , radius , uav->x , uav->y )  &&  (turn  <  0))  II 
(clintersect (hor , a , b , cr . x , cr . y , radius , uav->x , uav->y )  &&  (turn  >  0))  ) 

#ifdef  DEBUG 

printf ("TRAJGEN:  Switching  to  turning  with  turn  param  set  to  0\n"); 

#endif 

uav->turnFlag  -  1; 

next_state  =  sign (cos (uav->psi)*q2.y  -  sin(uav->psi)*q2.x) ; 

> 

break; 

case  1: 

#ifdef  DEBUG 

printf ("TRAJEGN:  Turn  param  set  to  1,  i.e.  parameterized  turns\n"); 

#endif 

qTmp.x  =  uav->x  -  uav->currentSigma[0]  .x; 
qTmp.y  =  uav->y  -  uav->currentSigma[0]  .y; 
makeUnitVector(feqTmp) ; 

turn2  =  turndir (qTmp, ql ) ; 
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hor2  =  getlinevals(uav->currentSigma [0] ,uav->currentSigma[l] ,&a2,feb2); 

/**  check  to  see  if  we  are  too  far  away  from  path  */ 
d  =  dist2path(uav->currentSigma[0] ,ql,uav->x,uav->y) ; 
if(  d  >=  df  ) 

{ 

#ifdef  DEBUG 

print f  ( "TRA JGEN :  Uav  far  off  the  path  with  turn  param  set  to  l\n"); 

#endif 

if  (  (clintersect(hor2,a2,b2, cl. x, cl. y, radius, uav->x,uav->y)  &&  (turn2  <  0))  II 

(clint er s ect (hor2 , a2 , b2 , cr . x , cr . y , radius , uav->x , uav->y )  &&  (tum2  >  0))  ) 
uav->turnFlag  =  5; 
else 

{ 

uav->turnFlag  =3; 

//  make  sure  that  our  normal  is  pointing  towards  the  path 

if (  sign( (uav->x-uav->currentSigma[0] .x)*ql .y  ~  (uav->y-uav->curr ent Sigma [0]  .y)*q 
normal_ang  =  atan2(ql .x,-ql .y) ; 
else 

normal _ang  =  atan2(-ql .x,qi .y) ; 
next_state  =  sign (sin (normal _ang  -  uav->psi)); 

> 

> 

//CHANGE:  there  has  to  be  a  better  way  to  make  sure  that  the  uav  angle 
//  is  close  to  match  the  line  it's  on 

beta  =  acos (qTmp . x*ql . x+qTmp . y*ql . y) ; 
if (fabs(beta)  >  10+PI/180  ){ 

#ifdef  DEBUG 

printf ("TRA JGEN:  Heading  far  off\n"); 

#endif 

break; 

> 

beta  =  f abs(PI  -  acos(q2.x*ql .x  +  q2.y*ql.y)); 
if(  Param~>equivdist  ) 

{ 

kappa  =  GetKappa(radius,beta,acc) ; 

> 

if (turn  ==  2) 

{ 

#ifdef  DEBUG 

printf(" TRA JGEN:  Calculating  U-turn\n"); 

#endif 

getUturncenter(uav->currentSigma[0] ,  uav->currentSigma[l] , radius, fee) ; 

> 
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else 

{ 

#ifdef  DEBUG 

printf  ("TRAJGEN:  Calculating  */.d  turn\n"  ,  turn); 

#endif 

getturncenter(uav->currentSigma[l] , ql,q2, radius, kappa, beta, &c) ; 

if  (  (ccintersect (uav, c , radius , cl , radius)  kk  (turn  >  0))  II 
(ccintersect (uav,c, radius, cr, radius)  kk  (turn  <  0))  ) 

#ifdef  DEBUG 

printf  ("TRAJGEN:  Starting  */,d  turn\n",  turn); 

#endif 

uav->turnFlag  =  6; 

//  printf  ("Kappa:  */,lf\nM  ,  kappa) ; 
if (  turn  >  0  ) 

{ 

v.x  =  cl.x  -  c.x; 
v.y  =  cl .y  -  c.y; 
n.x  =  -v.y; 
n.y  =  v.x; 

> 

else  if(  turn  <  0  ) 

{ 

v.x  =  cr.x  -  c.x; 
v.y  =  cr .y  -  c.y; 
n.x  =  -v.y; 
n.y  =  v.x; 

> 

else 

{ 

n.x  =  0; 
n.y  =  0; 

> 

//  QUESTION:  what  is  happening  here? 

next_state  -  sign(cos(uav->psi)*n.y  -  sin(uav->psi)*n.x) ; 

> 

if (turn  ==  0) 

#ifdef  DEBUG 

printf ("TRAJGEN:  Turn  is  equal  to  0\n"); 

#endif 

//  go  a  dist  radius/4  along  sigmal-sigmaO  toward  sigmaO 

//  if  that  point  is  not  on  the  same  side  of  simgal-sigma2  as  the  uav  is 

//  then  go  to  state  2 

ql.x  *  uav->currentSigma[l]  .x; 
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ql.y  =  uav->currentSigma[l]  .y; 
q2.x  =  uav->currentSigma[2] .x; 
q2.y  =  uav->currentSigma[2] .y; 
qTmp.x  =  uav->x ; 
qTmp.y  =  uav->y; 

s  =  getSideOf Line (feql , &q2 , &uav-> currents igma [0] ) ; 
if (!s) 

{ 

#ifdef  DEBUG 

print f ( "TRAJGEN :  The  line  from  wpl  to  wp2  is  the  same  as  vpO  to  vpl\n"); 

#endif 

//  its  the  same  line 
//  check,  x  =  b 
if (hor) 

s  =  sign(uav->currentSigma[0] . y-uav- > cur rent Sigma [1]  .y)  ; 
if(s  !=  sign(uav->y  -  uav“>currentSigma[l] .y)) 

{ 

#ifdef  DEBUG 

printf ( "TRAJGEN :  Switching  to  next  line  because  next  line  is  the  same  line"); 

#endif 

uav->turnFlag  =  2; 

> 

> 

else 

//  use  x  values  to  determine 

s  =  sign(uav->currentSigma[0]  ,x-uav->currentSigma[l] .x) ; 
if (s  !=  sign(uav->x  -  uav->currentSigma[l] .x)) 

{ 

#ifdef  DEBUG 

printf  ("TRAJGEN:  Switching  to  next  line  because  next  line  is  the  same  line"); 

#endif 

uav->turnFlag  =  2; 

> 

> 

> 

else 

{ 

of f setLine (&ql , &q2 , radius/4 . 0 , s) ; 
if (getSideOf Line (feql , &q2 , &qTmp)  ! *  s) 

{ 

#ifdef  DEBUG 

printf ("TRAJGEN:  Switching  to  next  line  because  turn  too  shallow\n") ; 

#endif 
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uav->turnFlag  =  2; 

> 

> 

> 

break; 
default : 

printf ("Unhandled  turn.param  =  */.d\n"  ,Param->turn_param)  ; 

> 

break; 

case  1: 

#ifdef  DEBUG 

printf ("TRAJGEN:  STATE  l\tAre  we  close  enough  to  the  next  path?\n"); 

#endif 

//  This  condition  determines  when  we  are  close  enough  to  the  next  path 
//  to  stop  turning 

next_state  =  sign ( cos (uav->psi)*q2.y  -  sin(uav->psi)*q2.x) ; 
if (  (uav-> state  !=  next_state)  II  (next.state  ==  0)  ) 
uav->turnFlag  =  2; 
break; 

case  2: 

#ifdef  DEBUG 

printf ("TRAJGEN :  STATE  2\tRequesting  new  waypoint \n" ) ; 

#endif 

if (  uav->newSigmaFlag  ==  0  ) 

{ 

uav->newSigmaFlag  =  1; 

else  if(  normNCwpNjUav^ current Sigma)  >  0.001)  /**<  this  next  elseif  happens  if  we 

{ 

uav->newSigmaFlag  =  0; 
uav->turnFlag  =0; 
sigmaCopy(wpN,uav->current Sigma) ; 

> 

break; 

case  3: 

#ifdef  DEBUG 

printf ("TRAJGEN:  STATE  3\tStarting  non-parameterized  turn\n"); 

#endif 

qTmp.x  =  cos(uav->psi) ; 
qTmp.y  =  sin(uav->psi) ; 
makeUnit Vector (feqTmp) ; 

turn2  =  turndir(qTmp,ql) ; 


101 


hor2  =  getlinevals(uav->currentSigma[0] ,uav->current Sigma [1] ,&a2,feb2); 

next.state  *  sign( (uav->x-uav->current Sigma [0] .x)*ql .y  -  (uav->y-uav->currentSigma[0] 

if  (  (clintersect(hor2,a2,b2, cl. x, cl. y, radius, uav->x,uav->y)  &&  (turn2  <  0))  II 
(clintersect (hor2 , a2 , b2 , cr . x , cr . y , radius , uav->x , uav->y)  &&  (turn2  >  0))  ) 
uav->turnFlag  =  5; 
else 

{ 

s  =  sign((uav->x-uav->currentSigma[0] .x)*ql .y  -  (uav->y-uav->currentSigma  [0] .y)*ql .x) 
//  make  sure  that  our  normal  is  pointing  towards  the  path 
if(  s  >=  0  ) 

{ 

normal _ang  =  atan2(ql .x,-ql .y) ; 
uav->endNormal . x  =  -ql . y ; 
uav->endNormal .y  =  ql.x; 

> 

else 

{ 

normal _ang  =  atan2(-ql .x,ql .y)  ; 
uav->endNormal . x  =  ql.y; 
uav->endNormal.y  =  -ql.x; 

> 

p  =  sign(sin(normal_ang  -  uav->psi)); 

//  determine  if  turn  is  finished 
if (  (p  !=  uav->state)  II  (p  ==  0)  ) 

uav->turnFlag  =  4; 
uav->startNormal.x  =  uav->x; 
uav->startNormal.y  =  uav->y; 

> 

> 

break; 

case  4; 

#ifdef  DEBUG 

print f ( "TRAJGEN :  STATE  4\tMiddle  of  non  parameterized  tura\n"); 

#endif 

qTmp.x  =  uav->endNormal .x; 
qTmp.y  =  uav->endNormal.y; 
makeUnitVector(feqTmp) ; 
turn2  =  turndir (qTmp , ql ) ; 

hor2  =  get line vals(uav-> current Sigma[0] ,uav->currentSigma[l] ,&a2,&b2); 
next _st ate  =  sign(cos(uav->psi)*ql .y  -  sin(uav->psi)*ql .x) ; 

if  (  (clintersect (hor2,a2,b2, cl. x, cl. y, radius, uav->x,uav->y)  &&  (turn2  <  0))  I  I 
(clintersect(hor2,a2,b2,cr.x,cr .y, radius, uav->x,uav->y)  &&  (tum2  >  0))  ) 
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uav->turnFlag  =5; 
break; 

case  5: 

#ifdef  DEBUG 

printf ("TRAJGEN:  STATE  5\tLast  part  of  non  parameterized  turn\n");  ~ 

#endif 

next.state  =  sign(cos(uav->psi) *ql .y  -  sin(uav->psi)*ql .x) ; 
if (  (uav->state  !=  next_state)  II  (next.state  ==  0)  ) 
uav->turnFlag  =  0; 
break; 

case  6: 

#ifdef  DEBUG 

printf ( “TRAJGEN :  STATE  6\tStarting  Kappa  turn\n"); 

#endif 

beta  =  fabs(PI  -  acos(q2.x*ql .x  +  q2.y*ql.y)); 
if(  Param->equivdist  ) 
kappa  =  GetKappa(radius,beta,acc) ; 

if (turn  ==  2)  getUtumcenter(uav->currentSigma[0] ,  uav- > current Sigma [1] ,  radius, fee); 
else 

getturncenter ( uav- > cur rent Sigma [1]  ,ql ,q2, radius , kappa, bet a, fee) ; 
if(  turn  >  0  ) 

{ 

v . x  =  cl.x  -  c.x; 
v.y  =  cl.y  -  c.y; 
n.x  «  -v.y; 
n.y  =  v.x; 

> 

else  if(  turn  <  0  ) 

v.x  =  cr.x  -  c.x; 
v.y  *  cr.y  -  c.y; 
n.x  =  -v.y; 
n.y  =  v.x; 

} 

else 

{ 

n.x  =  0; 
n.y  =  0; 

> 

next_state  =  sign(cos(uav->psi)*n. y  -  sin(uav->psi) *n.x) ; 
if (  (uav->state  !  =  next^state)  I  I  (next_state  ==  0)  ) 
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uav->turnFlag  =  7; 
next.state  =  0; 

> 

break; 

case  7 : 

#ifdef  DEBUG 

print f ( "TRA JGEN :  STATE  7\tMiddle  of  Kappa  turn\n"); 

#endif  #ifdef  UTURN.DEBUG 

printf ('‘Current  turn  is:  7*d\n"  ,turn) ; 
if (turn  ==  2){ 

printf  ( "  (7. . 31f  ,7. . 31f  )\t  (7. . 31f , 7. .  31f  ) \t  (7. . 31f ,% .  31f ) \n"  , 

uav->currentSigma[0] .x, 
uav->currentSigma[0] .y, 
uav->currentSigma[l] .x, 
uav->currentSigma[l] .y, 
uav->currentSigma[2] .x, 
uav->currentSigma[2] .y) ; 
printf (“State  7\nM); 

> 

#endif 

if  (  (clintersect(hor,a,b,cl.x,cl.y,radius,uav->x,uav->y)  &&  (turn  ==  1  I  I  (turn  ==  2 
(clintersect(hor, a, b,cr.x,cr.y, radius, uav->x,uav->y)  &&  (turn  ==  -1))  II 
(isLeft0fLine(&uav->currentSigma[0] ,&uav-> current Sigma[l] ,uav->x,uav->y)  &&  (turn  == 

#ifdef  UTUR1LDEBUG 
if (turn  ==  2){ 

printf  (“There  was  a  clintersect  with  %d,  7*.21f,  5£.21f,  (X.21f ,%.21f)\tuav  ->  (7*.21f,% 

> 

#endif 

if  (  (uav-> state  ==  0  &&  turn  !=  2)  I  I  (uav->state  ==  1)){ 

#ifdef  UTURN.DEBUG 

if (turn  ==  2 ){ 

printf ("Setting  uav->state  to  l\n“); 

> 

#endif 

next_state  =  1; 

> 

else  if (uav->state  ==  2) 

{ 

#ifdef  UTURN_DEBUG 

if (turn  «=  2){ 

printf ("Switching  to  state  8  now\n“); 

> 

#endif 

uav->turnFlag  =  8; 
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next.state  =  sign(cos(uav->psi)*q2.y  -  sin(uav->psi)*q2.x) ; 

> 

> 

else  if (uav->state  =-  lH 
#ifdef  UTURN.DEBUG 
if (turn  ==  2){ 

printf ("Setting  uav->state  to  2\nM); 

> 

#endif 

next.state  =  2; 

> 

else 

next.state  =  uav->state; 
break; 

case  8: 

#ifdef  DEBUG 

printf ("TRAJGEN:  STATE  8\tFinishing  Kappa  turn\n"); 

#endif 

//  This  condition  determines  when  we  are  close  enough  to  the  next  path 
//  to  stop  turning 

next.state  =  sign(cos(uav->psi)*q2.y  -  sin(uav->psi)*q2.x) ; 
if (  (uav-> state  !=  next.state)  ||  (next.state  ==  0)  ) 
uav->turnFlag  =  2; 
break; 

} 

#ifdef  DEBUG 

printf ("TRAJGEN:  — >  END  stateUpdate\n") ; 

#endif 

uav->state  =  next.state; 

> 

int  trajGenOutput (int  id,  double  vel,  double*  statevars,  double 
timestep,  planelnfo*  Param)  { 
int  i; 

static  double  last.psi [MAXP LANES] ; 
static  double  last .vel [MAXPLANES] ; 
static  int  init.derivative [MAXPLANES] ; 
static  int  allinit  =  1; 
uavlnfo  Kl; 

if (allinit) { 

f or(i=0;  KMAXPLANES;  i++) 
init.derivative [i]  -  1; 
allinit  =  0; 

> 
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statevars [0]  *  g_uav[id] .x; 
statevars [1]  =  g_uav[id]  .y; 
statevars [2]  «  g_uav[id] .psi; 
statevars [3]  =  vel; 

uavCopyCurrent (id,  &K1) ; 
trajGenDerivative(&Kl ,  vel,  Param) ; 

statevars [4]  =  Kl.x; 
statevars [5]  =  Kl.y; 
statevars [6]  =  Kl.psi; 
statevars [7]  =  Kl.Vel; 

if (init.derivative [id]  ){ 
last_psi[id]  =  Kl.psi; 
last_vel[id]  =  Kl.Vel; 
init_derivative [id]  =  0; 

> 

statevars [8]  =  K1 . Vel*cos(g_uav[id] .psi)  -  vel*(Kl .psi)*cos(g_uav[id] .psi) ; 

statevars [9]  =  K1 . Vel*sin(g_uav[id] .psi)  +  vel*(Kl .psi)*sin(g_uav[id] .psi) ; 

statevars [10]  =  (Kl.psi  -  last_psi [id] )/timestep; 
statevars [11]  =  (Kl.Vel  -  last_vel [id] )/timestep; 


last_psi[id]  -  Kl.psi; 
last^veltid]  =  Kl.Vel; 


return  g_uav[id] .newSigmaFlag; 

> 


void  traiGenDerivative(uavInf o*  uav,  double  vel,  planelnfo*  Param) 


vect3  cl , cr , ql , q2 , qTmp ; 

double  vectorNorm,  a,  b,  d,  psidot,  eps_d  = 
int  hor, turn, cl in, s; 

double  radius  =  vel/Param->psidot_max;  /**< 
double  df  =  2*radius; 

d.z  *  uav->x  +  radius*sin(uav->psi) ; 
cl.y  =  uav->y  -  radius*cos(uav->psi) ; 
cl.z  ~  uav->z; 


0.0001; 

radius  of  the  2  side  circles  of  the  plane  */ 
/**<  distance  that  is  considered  far  from 

/**<  x  coordinate  of  center  of  circle  on  1 
/**<  y  coordinate  of  center  of  circle  on  1 


cr.x  =  uav->x  -  radius*sin(uav->psi) ; 
cr.y  =  uav->y  +  radius*cos(uav->psi) ; 
cr.z  =  uav->z; 


/**<  x  coordinate  of  center  of  circle  on  r 
/**<  y  coordinate  of  center  of  circle  on  r 
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/**  ql  and  q2  are  unit  vectors  along  the  current  and  next  path  edges  respectively  */ 

ql.x  =  uav->curr ent Sigma [1] .x  -  uav->currentSigma[0] .x; 

ql.y  =  uav->currentSigma[l] .y  *  uav->currentSigma[0] .y ; 

ql.z  =  uav->z; 

makeUnitVector (feql) ; 


q2.x  =  uav->currentSigma[2] .x  -  uav-> currents igma [1] .x; 
q2.y  =  uav->currentSigma[2] .y  -  uav->currentSigma[l] .y; 
q2.z  =  uav->z; 
makeUnitVector (&q2) ; 


//  compute  distances 

hor  =  get 1 ine val s (uav->current Sigma [1]  ,uav->currentSigma[2]  ,&a,&b)  ; 
turn  =  turndir (ql ,q2) ; 

#ifdef  DEBUG 

printf  (“Derivative  turn  is:  */,d\n" ,turn) ; 

#endif 

d  =  dist2path(uav->currentSigma[0] ,ql,uav->x,uav->y) ; 

//  turn.. initiated 
switch  (uav->turnFlag) 

case  0: 

switch  (Param->turn_param) 

{ 

case  0: 

psidot  =  track (uav->currentSigma[0]  ,ql,vel,uav->x,uav->y,uav->psi,eps_d,Param->psidot 

if (turn  >  0  &fe  clintersectOior, a, b.cr.x.cr.y, radius, uav->x,uav->y)) 
psidot  =  Param->psidot_max; 

else  if  (turn  <  0  clintersectOior, a, b, cl. x, cl. y, radius, uav->x,uav->y)) 

psidot  =  ~Param~>psidot  joaax; 


break; 
case  1: 

psidot  =  track (uav->currentSigma [0] ,ql,vel,uav->x,uav->y,uav->psi,eps_d,Param->psidot 

break; 
default : 

printf  ("Unhandled  turn_param  =  %d"  , Par am->tum_param)  ; 

> 

break; 
case  1: 

if  (dist 2path (uav-> current Sigma [1]  , q2 , uav->x , uav->y )  <  eps_d) 
psidot  =  0; 
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else  if  (turn  >  0)  /**<  keep  turning  */ 

psidot  =  Param->psidot_max; 

else  if  (turn  <  0) 
psidot  =  -Param->psidot_max; 

else  /**<  otherwise  our  next  path  segment  is  straight  */ 
psidot  =  0; 
break; 

/**a  turn  is  not  finished  until  we  have  a  new  waypoint  */ 
case  2: 

psidot  =  track (uav->currentSigma [0] ,ql,vel,uav->x,uav->y,uav->psi,eps_d,Param->psidot 
break; 

case  3: 

/**  s  gives  the  direction  of  the  turn  -  in  the  direction  of  ql  */ 

s  =  sign( (uav->x  -  uav->currentSigma[0] .x)*ql .y  -  (uav->y  -  uav->currentSigma[0] .y)*q 

qTmp.x  -  cos(uav->psi) ; 

qTmp.y  =  sin(uav->psi) ; 

qTmp.z  =  uav->z; 

makeUnitVector(feqTmp) ; 

turn  =  turndir(qTmp,ql) ; 

hor  =  getlinevals (uav-> current Sigma [0]  , uav-> current Sigma [1] ,&a,&b); 

if  (turn  >  0)  /**<  turn  to  path  if  close  enough  */ 

if (  clint er sect (hor, a, b,cr.x,cr.y, radius, uav->x,uav->y)  ) 
psidot  =  Param->psidot_max; 

else  /**<  otherwise  turn  to  normal  path  */ 

psidot  =  s*Param->psidot__max; 

> 

else  if  (turn  <  0)  /**<  turn  to  path  if  close  enough  */ 

if (  clint er sect (hor , a , b , cl . x , cl . y , radius , uav->x , uav->y)  ) 
psidot  =  -Parana- >p  si  dot  _max ; 

else  /**<  otherwise  turn  to  normal  path  */ 

psidot  =  s*Param->psidot_max; 

> 

else 

psidot  =  track (uav->currentSigma [0] ,ql ,vel ,uav->x,uav->y ,uav~>psi,eps_d,Param->psidot_ir 
break; 

case  4: 

psidot  =  track  (uav->startNormal,uav“>endNormal,vel,uav-’>x,uav->y,uav->psi,eps_d,Parair 
break; 

case  5: 
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qTmp.x  =  uav->endNormal.x; 
qTmp.y  =  uav->endNormal .y ; 
makeUnitVector (ftqTmp) ; 
turn  =  turndir (qTmp ,  ql )  ; 
if (turn  >  0) 

psidot  =  Param->psidot_max; 

else  if (turn  <  0) 
psidot  =  -Param->psidot_max ; 
else 

psidot  =  0; 
break; 

case  6: 

if (turn  >  0) 

psidot  =  -Param->psidot_max ; 

else  if (turn  <  0) 
psidot  -  Para3n->psidot_max; 
else 

psidot  =  0; 
break; 

case  7: 

if (turn  >  0) 

psidot  =  Param->psidot_max; 

else  if (turn  <  0) 
psidot  =  -Param->psidot_max; 
else 

psidot  *  0; 
break; 

case  8: 

if (turn  >  0) 

psidot  =  -Param->psidot_max ; 

else  if (turn  <  0) 
psidot  =  Param->psidot_max; 
else 

psidot  =  0; 
break; 

default : 

printf  ("Unhandled  turn_initiated  =  */.d"  ,uav->turnFlag) ; 

> 

uav->x  =  vel*cos(uav->psi) ; 
uav->y  =  vel*sin(uav->psi) ; 
uav”>psi  =  psidot; 
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uav->Vel  =  0;  //10*(vel-uav->Vel) ; 

} 

//  returns  the  values  of  the  line  a,biny=ax+b,  and  hor  is  set  to  0 
//  if  the  line  is  x  =  b,  then  hor  is  set  to  1  and  b  is  returned 
int  getlinevals(vect3  pi,  vect3  p2,  double*  a,  double*  b)  { 
double  tol  =  0.0001; 

/**  The  tolerance  condition  is  necessary  to  prevent  truncation  errors 
*  for  nearly  parallel  lines  */ 
if(fabs(pl.x  -  p2.x)  <  tol) 

{ 

*a  =  0; 

*b  =  pl.x; 
return  1; 

} 

*a  =  (pi .y  -  p2.y)  /  (pl.x  -  p2.x); 

*b  =  ~(*a)*pl.x  +  pl.y; 
return  0; 


int  turndir(vect3  ql,  vect3  q2)  { 

//  if  unit  vectors  are  exactly  opposite,  just  turn 
if(  fabs(ql.x+q2.x)  <  0.0001  &&  f abs(ql .y+q2.y)  <  0.0001  ) 
return  2; 

/**  same  as  tmp  =  cross ([ql  0] , [q2  0]); 

*  turn  *  sign (tmp) ;  */ 

return  sign(  ql.x*q2.y  -  ql.y*q2.x  ); 

> 

int  sign (double  val)  { 
if(val  <  0)  return  -1; 
if (val  >  0)  return  1; 
return  0; 

> 

double  norm(vect3  q)  { 

return  sqrt(  q.x*q.x  +  q.y*q.y  ); 

> 

double  dist2path(vect3  p,  vect3  q,  double  X,  double  Y)  { 
vect3  v; 
double  proj_v; 

v.x  =  X  -  p.x; 
v.y  =  Y  -  p.y ; 
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v.z  =  p.z; 


pro  j  _v  =  v.x*q.x  +  v.y*q.y; 

v . x  -=  proj_v*q.x; 
v.y  -=  proj_v*q.y; 

return  norm(v) ; 

> 

double  track (vect3  p,  vect3  q,  double  v,  double  X,  double  Y, 
double  psi,  double  eps_d,  double  psidot _max,  planelnfo*  Param)  { 
double  alpha  =  10.0; 

double  psidot  =  alpha  * 

(  (2*psidot_max/PI)*atan(Param->kl*(q.y*(X  -  p.x)  -  q.x*(Y  -  p.y))) 
+  Param->k2*v*(cos(psi)*q.y  -  sin(psi)*q.x)  ); 

if(  psidot  >  psidot_max  ) 
psidot  =  psidot _max; 
else  if(  psidot  <  -psidot _max  ) 
psidot  =  -psidot_max; 

return  psidot; 

> 

//  returns  true  if  specified  circle  intersects  the  next  path  segment 
//  and  false  if  specified  circle  does  not  intersect  the  next  path  segment. 

// - - - 

//  hor  indicates  a  horizontal  path  segment  x  =  b,  otherwise  the  path  segment 

//  is  represented  as  y  =  ax  +  b 

//  xc  and  yc  are  the  x  and  y  coordinates  of  the  center  of  the  circle  to 
//  the  left  or  right  of  the  uav 
//  r  is  the  radius  of  that  circle 

//  X  and  Y  are  the  current  x  and  y  position  of  the  uav 
int  cl intersect (int  hor, double  a,  double  b,  double  xc,  double  yc, 
double  r,  double  X,  double  Y)  { 
double  cival ; 

if (hor)  //  a  horizontal  path  segment 

{ 

if (  f abs(xc  -  b)  <=  r  ) 
return  1; 
else 

return  0; 

> 

else 
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{ 


//  this  messy  expression  is  a  discriminant 

cival  =  (  (2*a*(b-yc)-2*xc)*(2*a*(b-yc)-2*xc)  -  4*(1  +  a*a)*(xc*xc  +  (b-yc)*(b-yc)  -  r* 
if (cival  >=  0)  4 

return  1; 
else 

return  0; 

} 

return  0; 

> 

/**  this  takes  the  norm  of  a  N  point  waypoint  path 
*  basically  it’s  just  to  see  if  the  path  has  changed  */ 
double  normN  (vect3  s  []  ,  vect3  t  []  )  { 
int  i  =  0; 
double  rval  =  0.0; 

for(;i<N;i++) 

{ 

rval  +=  (s  [i]  . x-t  [i]  . x)  *  (s  [i]  . x-t  [i]  . x)  ; 
rval  +=  (s[i]  .y-t[i]  .y)*(s[i]  .y-t  [i]  .y) ; 

/*  rval  +=  (s  [i]  .  z-t  [i]  .  z)  *  (s  [i]  .  z-t  [i]  .  z)  ;  */ 

> 

return  sqrt (  rval  ) ; 

> 

void  sigmaCopy(vect3  fromS[],  vect3  toS[])  { 
int  i  =  0; 

for ( ;i<N; i++) 

{ 

toS[i].x  =  fromS [i] .x; 
toS [i] .y  =  fromS [i] .y; 
toS [i]  .z  «  fromS [i]  .z; 

> 

> 

//  returns  the  angle  between  the  path  formed  by  the  next 
//  three  waypoints  in  the  order  wO,  wl,  w2 
double  getBeta(vect3*  wO,  vect3*  wl,  vect3*  w2){ 

/**  ql  and  q2  are  unit  vectors  along  the  current  and  next  path  edges  respectively  */ 
vect3  ql,  q2; 

ql.x  =  wl->x  -  w0->x; 
ql.y  =  wl->y  -  w0->y; 
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makeUnitVector(&ql)  ; 
q2.x  =  w2->x  -  wl->x; 
q2.y  =  w2->y  -  wl->y; 
makeUnitVector (&q2) ; 

return  fabs(PI  -  acos(q2.x*ql.x  +  q2.y*ql.y)); 


double  GetKappa (double  r,  double  beta,  int  n)  { 
int  i  =  0; 
double  L; 
double  a  =  0.0; 
double  b  =  1.0; 
for( ;i<n; i++) 

L  -  Lambda(r,(a  +  b)/2.0,beta) ; 
if (L  ==  0) 

return  (  (a  +  b)/2.0  ); 
else  if(L  >  0) 
b  =  (a  +  b)/2. 0; 
else 

a  =  (a  +  b)/2.0; 

> 

return  a; 

} 


double  Lambda (double  r,  double  k,  double  beta)  { 
double  sb  =  sin (beta/2 .0) ; 
double  sp  -  1  +  sb; 
double  sm  =  1  -  sb; 

return  (  r  *  (  sqrt(4.0  -  (sp  +  k*sm)*(sp  +  k*sm)  )  +  (1.0  +  k*(1.0/sb  -  1 .0))*cos (beta/2 
+  (beta  -  PI)/2.0  -  2.0*acos((sp  +  k*sm)/2.0)  )  ); 

} 

void  getturncenter(vect3  p,  vect3  ql,  vect3  q2,  double  radius, 
double  kappa,  double  beta,  vect3*  c)  { 
vect3  v; 

double  vectorNorm; 

v.x  =  q2.x  -  ql.x; 
v.y  *  q2.y  -  ql.y; 

vectorNorm  =  norm(v); 
if(  vectorNorm  ==  0  ) 
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{ 

v.x  =  0; 
v.y  =  0; 

> 

else 

{ 

v.x  =  v.x/vectorNorm; 
v.y  =  v.y/vectorNorm; 

> 

c~>x  =  p.x  +  radius* (  1.0  +  kappa* (1/sin (beta/2.0)  -  1)  )*v.x; 
c->y  =  p.y  +  radius* (  1.0  +  kappa* (1/sin (beta/2 . 0)  -  1)  )*v.y; 


int  ccintersect (uavlnf o*  uav,  vect3  cl, double  rl,vect3  c2, double 
r2)  { 

vect3  v,  begining,  ending,  intercept; 
double  vectorNorm,  dist,  x,  y; 
int  sx,  sy; 

//if (cl .x  ==  c2.x  kk  cl.y  ==  c2.y) 

if (  (fabs(cl.x  -  c2.x)  <  0.0001)  &&  (fabs(cl.y  -  c2.y)  <  0.0001)  ) 
{ 

//if (  rl  ==  r2  ) 

if(  fabs(rl-r2)  <  0.0001  ) 
return  1; 
return  0; 

> 

else 

{ 

v.x  =  cl.x  -  c2.x; 
v.y  =  cl.y  -  c2.y ; 
vectorNorm  =  norm(v) ; 

if(  vectorNorm  <=  (rl  +  r2)  ) 

{ 

//if(  rl  ==  r2  ) 

if(  fabs(rl-r2)  <  0.0001  ) 
return  1; 

else  if(  rl  <  r2  ) 

v.x  =  1.0  +  rl/vectorNorm*v.x; 
v.y  =  1.0  +  rl /vectorNorm* v.y; 
if (  norm(v)  >=  r2  ) 
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return  1 ; 
return  0; 

> 

else 

{ 

v.x  -  1.0  +  r2/vectorNorm*v.x; 
v.y  =  1.0  +  r2/vectorNorm*v.y; 
if(  norm(v)  >=  rl  ) 
return  1; 
return  0; 

> 

> 

else  //  we're  possibly  off  the  path 

{ 

return  (  offPathCCintersect(uav,cl,rl,c2)  ); 

> 

> 

return  0; 

> 

void  uav_prev_Copy(int  id)  { 
int  i  =  0; 

g_uav_prev  [id]  .  x  =  g_uav  [id]  .  x ; 
g_uav_prev [id] .y  =  g_uav[id] .y; 
g_uav_prev [id] . z  =  g_uav[id].z; 
g_uav_prev [id] .Vel  =  g_uav[id] .Vel; 
g_uav_prev [id] .psi  =  gjuav[id] .psi; 
g_uav_prev [id] .turnFlag  =  g_uav[id] .turnFlag; 
g_uav_prev  [id]  .  state  =  g_uav  [id]  .  state ; 

for(;i<N;i++) 

{ 

g__uav_prev  [id] . currentSigma[i] .x  =  g_uav[id] . cur rent Sigma [i] .x; 
g_uav_prev [id] . current Sigma [i] .y  -  g_uav[id] . currentSigma[i] . 
g_uav_prev [id] . cur rent Sigma [i] . z  =  g_uav [id] . cur rent Sigma [i] . 

> 

g„uav_prev [id] .newSigmaFlag  =  g_uav[id] .newSigmaFlag; 

g__uav_prev  [id]  .  startNormal .  x  =  g_uav  [id]  .  startNormal .  x ; 
g.uav.prev [id] . start Normal .y  =  g.uav [id] . startNormal .y; 
g_uav_prev [id] . startNormal . z  —  g_uav[id] . start Normal. z; 

g_uav_prev [id] . endNormal . x  =  g_uav [id] . endNormal . x ; 
g_uav_prev [id] . endNormal . y  «  g_uav [id] .endNormal. y; 
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N  ^ 


g_uav_prev [id]  .endNormal.z  =  g_uav[id]  .endNormal.z; 

> 

void  trajGenDestroyO  { 

PLANE_PTR  =  0; 

> 

//  the  old  ’get  distance  to  point’  function  from  robot  soccer  —  Crankshaft  rules 
double  getDistToLineAndIntersection(vect3*  beg,  vect3*  end,  vect3* 
p,  vect3*  cept){ 

double  mdest,  mpoint; 
double  ydiff,  xdiff; 
double  ydiffdest,  xdiffdest; 
double  bdest,  bpoint; 

ydiffdest  =  end->y  -  beg->y; 
xdiffdest  =  end->x  -  beg->x; 

if (! xdiffdest)  mdest  =  100000; 
else 

mdest  =  ydiff dest/xdiffdest ; 

if (! mdest)  mdest  =  0.00001; 
mpoint  =  -1/mdest; 

bdest  *  beg->y  -  mdest *beg->x; 

bpoint  =  p->y  -  mpoint*p->x; 

cept->x  =  (bpoint  -  bdest) /(mdest  -  mpoint); 
cept->y  =  (mdest *cept->x)  +  bdest; 

ydiff  =  p->y  -  cept->y; 
xdiff  =  p->x  -  cept->x; 

return  sqrt (ydiff *ydiff+xdiff*xdiff ) ; 

> 

void  makeUnitVector(vect3*  v)  { 
double  vectorNorm  =  norm(*v); 
if(  vectorNorm  ==  0  ) 

v->x  =  0; 

v->y  -  0; 

} 

else 


116 


{ 

v->x  =  v->x/vectorNorm; 
v->y  =  v->y/vectorNorm; 

> 

> 

//  for  a  vector  with  beginning  point  and  ending  point  ,e>  and  a  point  'p* 
//  this  function  returns  -1  for  ’p*  on  the  right  of  the  vector,  1  for  ,p>  on 
//  the  left  of  the  vector  and  0  for  ’p*  on  the  vector 
int  getSideOf Line (vect3*  b,  vect3*  e,  vect3*  p)  { 

return  sign(  (e->x-b->x)*(p->y-b*->y)  ~  (e->y”b->y)*(p->x-b->x)  ); 

> 

//if  the  uav  is  flying  too  far  off  the  path  to  intersect  the  circle 
//  then  this  function  is  called.  It  calculates  the  point  where  the 
//  circles  should  intersect,  then  it  creates  a  line  from  that  point 
//  perpendicular  to  the  current  path  segment  and  checks  to  see  if  the 
//  circle  has  crossed  the  line.  If  it  has  it  returns  true 
int  offPathCCintersect(uavInfo*  uav,  vect3  c,  double  r,  vect3 
sideCircle)  { 

vect3  begining, ending, intercept ; 
double  dist, gamma; 
int  side; 

//  set  the  line  ’ begining  to  end*  to  the  current  line 
//  the  plane  is  on 

begining. x  =  uav~>currentSigma[0] .x; 
begining. y  =  uav->currentSigma[0] .y; 
ending. x  =  uav->current Sigma [1] .x; 
ending. y  =  uav->currentSigma[l] .y; 
begining. z  =  ending, z  =1; 

side  =  getSideOf Line (febegining, tending, &uav->currentSigma [2] ) ; 
if (! side)  return  0; 

//  offset  to  opposite  side  of  sigma2 
of f  setLine (febegining , tending , r , -s ide ) ; 

//  get  the  distance  to  the  offset  line  and  the  point  of  intersection 
dist  =  getDistToLineAndlntersect ion (febegining,  feending,  fee,  feint ercept ) ; 

//  compute  the  distance  needed  to  offset 
gamma  =  sqrt(4*r*r  -  dist*dist) ; 

side  =  getSideOf Line (fee, feintercept, febegining) ; 

//  offset  to  the  side  of  the  begining  point 
off  setLine (fee , feintercept , gamma, side) ; 
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//  the  center  of  the  side  circle  has  crossed  the  line, 

//  then  we  need  to  start  turning 

if(  getSideOfLine(&c,feintercept,&sideCircle)  !=  side  ) 

{ 

#ifdef  DEBUG 

printf  ('’Returning  1  from  of fCCintersect !\n") ; 

#endif 

return  1; 

> 

#ifdef  DEBUG 

printf ("Returning  0  from  of fCCintersect !\n") ; 

#endif 
return  0; 

> 

//  this  function  takes  a  line  defined  by  a  starting  and  ending  point 
//  and  moves  it  a  distance  ’dist’  to  the  right  or  the  left  side  of 
//  the  line,  ’b’  and  >eJ  are  changed  to  the  new  offset  line 
void  of f setLine (vect3*  b,  vect3*  e,  double  dist,  int  side)  { 

//  side  =  1  for  left,  side  -  -1  for  right 
double  x,y,xoffset ,yoff set ; 

x  =  e->x  -  b“>x; 
y  =  e->y  -  b->y; 

//  now  offset  the  line  a  distance  of  Mist*  to  side  'side* 
xoff set  =  dist*y/sqrt (x*x+y*y) ; 
yoff set  =  dist*x/sqrt(x*x+y*y) ; 

b->x  +=  -side*xoffset ; 
e->x  +=  -side*xoff set ; 
b->y  +=  side*yoff set ; 
e  >y  +=  side*yoff set ; 

> 

//  this  function  figures  out  where  the  center  of  the  turning  circle  should 
//  be  given  that  the  path  calls  for  doubling  back  on  itself  -  i.e.  a  U  turn 
//  this  will  preserve  path  length 

void  getUturncenter (vect3  sigmaO,  vect3  sigmal,  double  r,  vect3* 
c)  { 

double  a,b,qa,qb,qc; 
int  hor,s; 

//  dist  is  the  distance  from  the  double-back  point 
//  that  the  center  of  the  turn  circle  needs  to  be 
//  placed.  We  figured  this  out  with  the  geometry  of 
//  three  equal  radius  circles  touching 
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double  dist  =  (7.0/6.0*PI  -  sqrt(3))*r; 

hor  =  getlinevals(sigmaO,sigmal ,&a,&b) ; 

if(hor)  //  line  equation  is  x  =  b; 

{ 

c->x  =  sigmal.x; 
s  =  sign(sigma0.y-sigmal .y) ; 
c->y  =  sigmal.y  +  s*dist; 
return; 

> 

//  line  equation  is  y  =  ax  +  b 

//  this  is  the  solution  to  the  set  of  equations: 

//  y  =  ax  +  b 

//  dist~2  =  (x-sigmal .x)“2  +  (y-sigmal .y) *2 

//  put  into  quadratic  variables 
qa  =  l+a*a; 

qb  =  -2* (sigmal.x  -  a*b  +  a*sigmal.y); 

qc  =  sigmal .x* sigmal .x  +  sigmal .y*sigmal .y  +  b*b  -2*b*sigmal .y  -  dist*dist; 
c->x  =  (-qb  +  sqrt (qb*qb  -  4*qa*qc)  )  /  (2*qa) ; 
s  =  sign(sigma0.x  -  sigmal.x); 

if(  s  !=  sign(c->x  -  sigmal.x)  )  //  if  we  went  the  wrong  way 

c->x  =  (-qb  -  sqrt(qb*qb  -  4*qa*qc)  )  /  (2*qa) ; 

c->y  =  a*c->x  +  b; 

> 

void  uavCopyCurrent ( int  id,  uavlnfo*  K)  //  copies  g_uav_prev  to 
K.  This  so  iterations  in  the  algorithm  { 

//  don’t  mess  up  g_uav_prev  We  use  the  previous  g_uav  information 

int  i;  //to  match  Matlab  sequence  and  to  avoid  making  a  dependent 

//  order  of  calls  to  rungkut  and  stateUpdate 

K->x  =  g_uav[id].x; 

K->y  =  g_uav [id] .y; 

K->z  =  g_uav[id].z; 

K->psi  =  g_uav[id] .psi; 

K->Vel  =  g_uav [id] .Vel; 

K->turnFlag  =  g_uav[id] .turnFlag; 

K->state  =  g_uav [id] .state; 

K->newSigmaFlag  =  gjuav[id] .newSigmaFlag; 

for(i=0; i<N; i++) 

{ 

K->currentSigma[i] .x  =  g_uav[id] . cur rent Sigma [i] .x; 

K->currentSigma[i] .y  =  g_uav[id]  . current Sigma [i] .y; 
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K->currentSigma[i] .z  -  g_uav[id]  .currentSigma[i]  .z; 

> 

K-> st art Normal . x  =  g_uav[id] .startNormal .x; 

K-> st art Normal .y  =  g_uav [id] . startNormal. y; 

K-> st art Normal . z  =  g_uav [id] . startNormal . z ; 

K->endNormal .x  =  g_uav[id] .endNormal.x; 

K->endNormal .y  =  g_uav[id]  .endNormal.y; 

K->endNormal.z  =  g_uav[id] .endNormal.z; 

> 

int  isLef tOf Line (vect3*  pi,  vect3*  p2,  double  x,  double  y)  { 

if (  sign(  (p2->x-pl->x) * (y-pl->y)  -  (p2->y~pl->y)*(x-pl->x))  ==  1  ) 
return  1; 
return  0; 

> 

//  computes  the  'delta*  function  on  page  45  of  Erik  Anderson's  thesis. 

//  This  is  the  distance  to  complete  half  of  a  kappa  turn.  This  is  used 

//  to  tell  if  a  path  segment  is  too  short  -  i.e.  if  half  the  distance  of 

//  the  turn  into  the  path  segment  plus  half  of  the  distance  of  the  turn 

//  out  of  the  path  segment  is  longer  than  the  path  segment  length,  then 
//  the  length  of  the  segment  is  too  short, 
double  half KappaDist (double  kappa,  double  beta,  double  r)  { 
double  dist; 

if  (beta  <  0.0001H  //  u-turn 

return  r*(7.0/6.0*PI  -  sqrt(3))/2.0; 

> 

if(  fabs(beta  -  PI)  <  0.0001){  //no  turn 
return  0; 

> 

dist  =  1  +  sin(beta/2)  +  kappa*(l  -  sin(beta/2) ) ; 
dist  *=  dist; 
dist  =  4  -  dist; 

if  (dist  <  0) 
dist  =0; 
else{ 

dist  =  sqrt(dist); 
dist  *=  r; 

> 

dist  +=  r*(l  +  kappa* (1.0/sin (beta/2)  -  1) )*cos (beta/2) ; 
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return  dist; 

>  /* * 

*  File:  traj_states . c 

*  Created:  Tue  Dec  3 

* 

*/ 

#def ine  S_FUNCTI ON_NAME  traj.states  #define  S_FUNCTION_LEVEL  2 

/*  N  is  the  number  of  waypoints  passed  to  trajGen  */  #define  N  3 

#def ine  NUM_INPUTS  1  #define  INPUT.WIDTH  3*N+1 

#def ine  INPUT_FEEDTHROUGH  1 

#def ine  NUM.OUTPUTS  1  tdefine  OUTPUT. WIDTH  14 

#define  NP ARAMS  2 

#def ine  NUM_DISC_STATES  0  #define  NUM_CONT_STATES  0 

#def ine  SAMPLE.TIME  INHERITED_SAMPLE_TIME 
#include  "simstruc.h"  #include  "trajGen. h" 

#def ine  UAVDATA  0  #define  UAVDATA.PARAM(S)  ssGetSFcnParam(S, 
UAVDATA) 

#def ine  INITWPS  1  #define  INITWPS_PARAM(S)  ssGetSFcnParam(S, 
INITWPS) 

void  fillInParam(const  mxArray*  p,  planelnfo*  Param) ; 

/*====================* 

*  S-function  methods  * 

*====================*/ 

#def ine  MDL_CHECK_PARAMETERS  #if  defined (MDL_CHECK_PARAMETERS)  && 
defined (MATLAB_MEX_FILE)  /*  Function:  mdlCheckParameters 


*  Abstract: 

*  Validate  our  parameters  to  verify  they  are  okay . 
*/ 

static  void  mdlCheckParameters (SimStruct  *S)  { 
const  mxArray  *paramVal; 
mxArray  *p; 


double  *pr; 


paramVal  =  UAVDATA.PARAM(S) ; 

if ( ! mxIsStruct (paramVal) ) { 

ssSetErrorStatus(S, "Parameter  is  not  a  struct!"); 
return ; 

> 

p  =  mxGetField(paramVal,0, "ic") ; 

if (p  ==  NULL  | | 

!mxIsNumeric(p)  I  I 
!mxIsDouble(p)  I  I 
mxIsLogical(p)  I  I 
mxIsComplex(p)  I  I 
mxIsSparse(p)  I  I 
mxIsEmpty(p)  I  I 
mxGetN (p) *mxGetM (p)  !=  6  ) 

ssSetErrorStatus (S , "wrong  dimensions! ") ; 
return; 

> 

p  =  mxGetField(paramVal,0, "psidot.max") ; 

if (p  ==  NULL  I  I 

ImxIsNumericCp)  I  I 

!mxIsDouble(p)  I  I 

mxIsLogical(p)  II 

mxIsComplex(p)  I  I 

mxIsSparse(p)  II 

mxIsEmpty(p)  I  I 

mxGetN (p)*mxGetM(p)  !=  1  ) 

{ 

ssSetErrorStatus (S, "Error  in  field  'psidot .max' ! ") 
return ; 

> 

p  =  mxGetField(paramVal,0, "turn_param") ; 

if (p  ss  NULL  II 

!mxIsNumeric(p)  II 

!mxIsDouble(p)  I  I 

mxIsLogical(p)  I  I 

mxIsComplex(p)  I  I 

mxIsSparse(p)  I  I 

mxIsEmpty(p)  I  I 

mxGetN (p)*mxGetM(p)  !=  1  ) 
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ssSetErrorStatus (S, "Error  in  field  ’turn.param’ ! ") ; 
return; 

} 

p  =  mxGetField (paramVal, 0, "equivdist") ; 

if (p  ==  NULL  I  I 

!mxIsNumeric(p)  II 

ImxIsDouble (p)  I  I 

mxIsLogical(p)  II 

mxIsComplex(p)  I  I 

mxIsSparse(p)  II 

mxIsEmpty(p)  I  I 

mxGetN(p)*mxGetM(p)  !=  1  ) 

{ 

ssSetErrorStatus (S, "Error  in  field  'equivdist * ! ") ; 
return ; 

> 

p  =  mxGetField(paramVal,0, "kappa") ; 

if(p  ==  NULL  II 

!mxIsNumeric(p)  I  I 

ImxIsDouble (p)  I  I 

mxIsLogical(p)  I  I 

mxIsComplex(p)  I  I 

mxIsSparse(p)  II 

mxIsEmpty(p)  I  I 

mxGetN(p)*mxGetM(p)  !-  1  ) 

{ 

ssSetErrorStatus (S, "Error  in  field  'kappaM"); 
return ; 

> 

paramVal  =  INITWPS.PARAM(S) ; 

if (paramVal  ==  NULL  I  I 

ImxIsNumeric (paramVal)  I  I 

ImxIsDouble (paramVal)  I  I 

mxIsLogi cal (paramVal)  I  I 

mxIsComplex (paramVal)  I  I 

mxIsSparse (paramVal)  I  I 

mxIsEmpty (paramVal)  I  I 

mxGetN (par amVal)*mxGetM (paramVal)  !=  3*N  ) 

{ 

ssSetErrorStatus (S, "Error  with  parameter  ’wpsOM"); 
return; 
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> 


#endif  /*  MDL_CHECK_PARAMETERS  */ 
/*  Function:  mdllnitializeSizes 


*  Abstract: 

*  Setup  sizes  of  the  various  vectors. 

*/ 

static  void  mdllnitializeSizes (SiraStruct  *S)  { 

ssSetNumSFcnParams (S ,  NP ARAMS) ;  /*  Number  of  expected  parameters  */ 

#if  defined (MATLAB.MEX.FILE) 

if  (ssGetNumSFcnParams (S)  ==  ssGetSFcnParamsCount (S))  { 
mdlCbeckParameters(S) ; 
if  (ssGetErrorStatus(S)  !=  NULL)  { 
return ; 

> 

}  else  { 

return;  /*  Parameter  mismatch  will  be  reported  by  Simulink  */ 

> 

#endif 

ssSetNumCont States (S ,  NUM_C0NT_ STATES) ; 
ssSetNumDiscStates(S,  NUM_DISCLSTATES) ; 

if  ( ! ssSetNumInputPorts(S,  NUM_INPUTS))  return; 
ssSetInputPortWidth(S,  0,  INPUT  J/IDTH) ; 

ssSetlnputPortRequiredContiguous (S,0, 1) ;  /*  so  that  we  can  access  using 

*  ’ssCetlnputPortRealSignal’  */ 
ssSetlnputPortDirectFeedThroughCS ,  0,  INPUT_FEEDTHROUGH) ; 

if  ( ! ssSetNumOutputPort s (S ,  NUM_0UTPUTS) )  return ; 
ssSetOutputPortWidth(S,  0,  0UTPUT_WIDTH) ; 

ssSetNumSampleTimesCS,  1);  /*only  one  sample  time  */ 

ssSetNumRWork(S,  2);  /*id  for  trajGen,  previous  time  we  calculated*/ 

ssSetNumIWork(S,  0); 

ssSetNumPWork(S,  1);  /*param  struct  for  trajGen  */ 

ssSetNumModes(S ,  0); 
ssSetNumNonsampledZCs(S,  0); 

ssSetSFcnParamNotTunable (S , UAVDATA) ;  /*  parameters  are  not  tunable  -  i.e.  wont 

*  change  during  simulation  */ 

ssSetSFcnParamNotTunable (S, INITWPS) ;  /*  parameters  are  not  tunable  -  i.e.  wont 

*  change  during  simulation  */ 
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/*  Take  care  when  specifying  exception  free  code  -  see  sf untmpl.doc . c  */ 
/*  ssSetOptions (S ,  SS_OPTION„EXCEPTION.FREE_CODE) ;  */ 
ssSetOptions(S,  0); 


/*  Function:  mdllnitializeSampleTimes 


*  Abstract: 

*  Specif iy  the  sample  time. 

*/ 

static  void  mdllnitializeSampleTimes (SimStruct  *S)  { 
ssSetSampleTime(S,  0,  SAMPLE_TIME) ; 
ssSetOff setTime(S,  0,  0.0); 

> 

#def ine  MDL.START  #if  defined (MDL.START) 

/*  Function:  mdlStart  -==”===========================”=:===--======:=:=-==== 

*  Abstract : 

*  This  function  is  called  once  at  start  of  model  execution.  If  you 

*  have  states  that  should  be  initialized  once,  this  is  the  place 

*  to  do  it. 

*/ 

static  void  mdlStart (SimStruct  *S)  { 

/*  set  up  param  */ 

ssGetPWork(S) [0]  =  malloc(sizeof (planelnfo)) ; 
f illInParam(UAVDATA_PARAM(S) , (planelnfo*)  (ssGetPWork(S)  [0] )) ; 

/*  call  trajGen  initialize  function  */ 

ssGetRWork(S) [0]  =  trajGenInitialize( (planelnfo*)  (ssGetPWork(S) [0] ) ,  mxGetPr (INITWPS.PAR, 
ssGetRWork(S) [1]  =0.0; 

>  #endif  /*  MDL_ START  */ 

static  void  mdlOutputs (SimStruct  *S,  int_T  tid)  { 
double  psidotd,psid,vel,  curTime, time step; 
double  statevars [12] ; 
int  request_new_sigma; 
vect3  curPos,wpN[3] ; 

const  real_T  *u  =  ssGetInputPortRealSignal(S,0) ; 
realJT  *y  =  ssGetOutputPortRealSignal(S ,0) ; 

planelnfo*  Param  =  (planelnfo*)  (ssGetPWork(S) [0] ) ; 
int  id  =  (int)  (ssGetRWork(S) [0] ) ; 

vel  =  u[0]  ; 
wpN  [0]  .x  =  u[l]  ; 
wpN [0]  .y  =  u[2]  ; 
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wpN[0]  .z  =  u[3]  ; 
wpN[l]  .x  =  u[4]  ; 
wpN[l]  .y  =  u[5]  ; 
wpN  [1]  .z  =  u[6]  ; 
wpN  [2]  .  x  =  u  [7]  ; 
wpN[2]  .y  =  u[8]  ; 
wpN  [2]  .z  =  u[9]  ; 

curTime  =  ssGetT(S); 

timestep  =  curTime-ssGetRWork(S) [1] ; 

ssGetRWork(S) [1]  =  curTime; 

/*  call  trajGen  update  and  output  functions  */ 

reque st _new_ sigma  =  t raj GenOutput (id, vel, statevars,  time step, Par am) 

curPos.x  =  statevars[0] ; 

curPos.y  =  statevars [1]  ; 

psid  *  statevars [2] ; 

psidotd  =  statevars [6] ; 

tr aj  GenUpdat e ( id , vel , wpN , Param , timestep) ; 


y[0] 

= 

(double)  reque st _new_ sigma; 

y[l] 

= 

Param->ic.zO; 

y[2] 

curPos.x; 

y  [3] 

= 

curPos.y; 

y[4] 

as 

psid ; 

y[5] 

= 

vel; 

y[6] 

ss 

statevars [4] ; 

//xdotd 

y[7] 

= 

statevars  [5]  ; 

//ydotd 

y[8] 

= 

statevars [6] ; 

//psidotd 

y[9] 

= 

statevars [7] ; 

//vdotd 

y[10] 

= 

statevars [8]  ; 

//xdotdotd 

yCli] 

= 

statevars  [9]  ; 

//ydotdotd 

y[i2] 

= 

statevars  [10]  ; 

//psidotdotd 

y[i3] 

= 

statevars  [11]  ; 

//vdotdotd 

/*  Function:  mdlTerminate 


*/ 

static  void  mdlTerminate (SimStruct  *S)  { 
/*  call  trajGen  destroy  function  */ 
trajGenDestroyO ; 
free (ssGetPWork(S) [0] ) ; 

> 
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void  f ill InParam( const  mxArray*  p,  planelnfo*  Param)  { 
mxArray*  f  =  mxGetField(p,0, Hic") ; 
double*  ic  »  mxGetPr(f); 

Param->ic .xO  -  ic[0]; 

Param->ic.yO  =  ic  [13 ; 

Param->ic .zO  =  ic [4] ; 

Param->ic .psiO  -  ic  [2] ; 

Param~>ic.zdotO  =  ic  [5] ; 

Param->ic. vO  =  ic[3]; 

f  =  mxGetField(p,0,"psidot_max") ; 

Param->psidot_max  =  mxGetScalar (f ) ; 

f  *  mxGetField(p,0, "turn_param") ; 

Param->turn_param  =  (int)  mxGetScalar (f ) ; 

f  =  mxGetField(p,0, "equivdist") ; 

Param->equivdist  =  (int)  mxGetScalar(f ) ; 

f  =  mxGetField(p,0, "kappa") ; 

Param-> kappa  *  mxGetScalarCf ) ; 

Param->kl  =  10.0; 

Param->k2  =  1.3; 

> 

#ifdef  MATLAB_MEX_FILE  /*  Is  this  file  being  compiled  as  a 
MEX-file?  */  #include  "simulink. c"  /*  MEX-file  interface 

mechanism  */  #else  #include  "cg_sfun.h"  /*  Code  generation 

registration  function  */  #endif 


/* - */ 

/*  Name:  cmnStructs.h  */ 
/*  Created:  8/5/2002  */ 
/* - */ 


/**  This  file  contains  all  the  common  structures 
*  to  be  used  in  sf unction. c 
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#ifndef  CMNSTRUCTS.H 


#def ine  CMNSTRUCTS_H 


#include  <list> 
#include  <math.h> 


#ifndef  FOR.WINDOWS 

#include  <pthread.h>  /**<  used  for  threads 

which  are  used  in  the  openGL  engine  */ 

#endif 


#def ine  N  3  /**<  N  is  the  number 

of  waypoints  the  traj.gen  sf unction  receives  */ 


typedef  struct  { 

double  x; 
double  y; 
double  z; 


/**<  x  coordinate 
/**<  y  coordinate 
/**<  z  coordinate 


of  vector  */ 
of  vector  */ 
of  vector  */ 


typedef  struct  { 


double  x; 
double  y; 
double  r; 

std: :list<vect3>  nodes; 

>  ThreatStruct ; 

typedef  struct  { 

/**<  initial  inertial  position  x  */ 
/**<  initial  inertial  position  y  */ 
/**<  initial  altitude  */ 

/**<  initial  heading  */ 

/**<  initial  climb  rate  */ 

/**<  initial  velocity  */ 

>  ic_t ; 

typedef  struct  { 


double  xO; 
double  yO; 
double  zO; 
double  psiO; 
double  zdotO; 
double  vO; 


ic_t  ic; 
double  psidot_max; 
int  turn.param ; 

int  equivdist; 

double  kappa; 

double  kl; 
double  k2; 

>  planelnfo; 

typedef  struct{ 
double  th_x; 
double  th_y; 
double  xd; 
double  yd; 
double  psid; 
double  vd; 
double  psidotd; 

}  at t Info; 


/**<  initial  conditions  */ 

/**<  turning  rate  limit  */ 

/**<  l=parameterized  turn  in  */ 

/**<  trajectory  generator  */ 

/**<  l=trajectories  have  same  */ 

/**<  length  as  Voronoi  path  */ 

/**<  parameter  for  trajectory  */ 

/**<  generator  */ 

/**<  kl  used  in  the  control  law  p.  47  of  thes 
/**<  k2  damping  factor  in  control  law  */ 
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/*******Global  Defines  for  the  entire  Program  ****************/ 

#def ine  PI  3.141592653589793  /**<  pi  -  could  be 

better  precision  */ 

#def ine  TIME.STEP  0.0005  /**<  global  time 

step  */ 

//#def ine  TIME.STEP  0.01 
#def ine  SEND.TIME  0.05 
#def ine  MAXPLANES  10 


/**  a 11  memory  allocations  are  freed  for  the  program  and  for  each 
individual 

*  sfunction  also  any  file  pointers  are  closed.  This  prototype  is  in  here  so  that 

*  every  file  can  have  access  to  this  function 
*/ 

void  simUavDestroyO  ; 

#endif 

/* - */  /*  Name:  t  raj  Gen.  h 

*/  /*  Created:  8/5/2002  */ 

/* - */ 

/**  This  file  contains  all  the  function  prototypes  and  globals 

*  to  be  used  in  sfunction. c 
*/ 

#ifndef  TRAJGEN.H  #define  TRAJGEN.H 

#include  <stdio.h>  #include  "cmnStructs ,hM  #include  <math.h> 

/***********  UAV  info  struct  ******************/  typedef  struct  { 

double  x;  /**<  current  x  position  of  the  plane  */ 
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double  y; 
double  z; 
double  Vel; 
double  psi; 


/**<  current  y  position  of  the  plane  */ 
/**<  current  z  position  of  the  plane  */ 
/**<  current  airspeed  V  */ 

/**<  current  heading  of  the  plane  */ 


int  turnFlag;  /**<  turn  initiaed  f lag 

*  0  for  not  turning 

*  1  for  turning 

*  2  for  just  finished  a  turn 

*  3  for  turning  to  a  path  normal  (when  fax  away  from  path) 

*  4  for  tracking  normal 

*  5  for  turning  from  normal  to  path 

*  6  for  first  segment  of  hit-waypoint  turn 

*  7  for  second  segment  of  hit-waypoint  turn 

*  8  for  third  segment  of  hit-waypoint  turn 
*/ 


int  state; 

vect3  currentSigma[N]  ; 
int  newSigmaFlag; 
vect3  startNormal; 
vect3  endNormal; 

>  uavlnfo; 


/**<  current  state  of  state  machine  */ 

/**<  current  value  of  sigma  */ 

/**<  request  for  new  sigma  */ 

/**<  starting  coordinate  of  the  normal  vector 
/**<  ending  coordinate  of  the  normal  vector  * 


/**  Initializes  all  the  structs  to  be  used  in  the  sfunction  */  int 
tr aj Genlnit ialize (plane Inf o*,  double*) ; 


/**  Destorys  any  memory  allocated  by  the  sfunction  and  closes  any 
file  pointers  */  void  trajGenDestroyO ; 

/**  Determines  the  output  of  the  sfunction  */  int 
trajGenOutput (int ,  double,  double*, double, planelnf o*) ; 

/**  Calls  both  the  state  update  and  the  Runge-Kutta  method  */  void 
trajGenUpdate(int ,  double,  ve ct 3*, plane Inf o*, double ) ; 

/**  Updates  the  discreet  variables  used  in  the  sfucntion  */  void 
stateUpdate (uavlnfo*,  double,  vect3*,  planelnf o*) ; 


/**  Derivative  used  in  the  s-function  */  void 
trajGenDerivative (uavlnfo* ,  double,  planelnf o*) ; 


/**  4th  order  runge-kutta  method  for  determining  trajectory 
generation  */  void  rungkutTrajGen(int,  double,  planelnf o* , double) ; 

/**  To  copy  g_uav  to  compute  psidotd  */  void 
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uavCopyCurrent (int ,uavlnf o*) ; 

/**  returns  the  values  of  the  line  a,b  in  y  =  ax  +  b,  and  hor  is 
set  to  0 

*  if  the  line  is  x  =  b,  then  hor  is  set  to  1  and  b  is  returned 
*/ 

int  getlinevals(vect3, vect3, double* , double*) ; 

/**  turn  specifies  the  direction  of  the  turn,  turn  is  positive 
for  a  ccw 

*  turn  and  negative  for  a  cw  turn.  0  is  returned  if  the  next  path  requires 

*  no  turn.  Currently,  we  are  on  the  path  specified  by  ql.  The  path  we 

*  are  switching  to  is  q2. 

*/ 

int  turndir (vect3,vect3) ; 

/**  returns  the  sign  of  a  number,  with  0  having  a  sign  of  0  */  int 
sign(double) ; 

/**  return  the  norm  of  a  vector  -  currently  only  in  2  dimensions 
(x  and  y)  */  double  norm(vect3) ; 

/**  p  is  the  starting  waypoint  of  the  path,  q  is  a  unit  vector  in 
the 

*  direction  of  the  path.  The  Euclidean  distance  between  the  path  and  the 

*  airplane  is  returned. 

*/ 

double  di st 2path(vect3,vect3, double , double) ; 

/** 

♦This  function  returns  a  value  for  psidot  for  tracking  a  path  seqment 

*  when  we  are  near  the  path. 

*  p  is  the  beginning  waypoint  of  our  path,  q  is  a  unit  vector  in  the 

*  direction  of  the  path.  eps_d  is  the  maximum  distance  from  the  path 

*  for  which  we  say  we  are  tracking  the  line. 

*/ 

double 

track (vect3 , vect3 , double , double , double , double , double , double , plane Inf o*) ; 

/* 

*  returns  ci  -  true  if  either  circle  intersects  the  next  path  segment 

*  and  ci  =  false  if  neither  circle  intersects  the  next  path  segment. 

*  hor,a,b  are  the  values  returned  from  getlinevals 
*/ 

int 

clintersect (int , double , double , double , double , double , double , double) ; 
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/**  this  takes  the  norm  of  a  N  point  waypoint  path 

*  basically  it’s  just  to  see  if  the  path  has  changed  */ 
double  normN(vect3  s  []  ,  vect3  t[]); 

/**  copys  a  set  of  N  waypoints  to  another  N  waypoint  path  */  void 
sigmaCopy(vect3  fromS[],  vect3  toS[]); 

//  returns  the  angle  between  the  path  formed  by  the  next 
//  three  waypoints  in  the  order  wO,  wl,  w2 
double  getBeta(vect3*  wO,  vect3*  wl,  vect3*  w2) ; 

/**  Returns  the  value  of  kappa  that  makes  the  length  of  the  path 
segment  and 

*  the  length  of  the  constrained  turn  equal  to  within  2~(-n)  where  n  is  a 

*  positive  integer. 

*/ 

double  GetKappa (double, double, int) ; 

/**  Returns  the  value  of  the  distance  between  the  path  segment  and 
the 

*  constrained  turn  for  the  angle  beta  between  the  successive  path  segments, 

*  the  value  k  (i.e.  kappa  which  lies  between  0  and  1),  and  the  turning 

*  radius  r 
*/ 

double  Lambda (double , double , double) ; 

/**  Returns  the  center  of  the  cicle  so  the  plane  passes  through  a 
waypoint . 

*  p  is  the  waypoint  to  pass  through,  ql  is  a  unit  vector  in  the  direction 

*  of  the  current  path  segment,  q2  is  a  unit  vector  in  the  direction  of 

*  the  next  path  segment,  and  radius  is  the  radius  of  the  turn. 

*  Assume  that  ql  and  q2  are  not  parallel. 

*/ 

void  getturncenter (vect3, vect3, vect3, double, double , double ,vect3*) ; 

/**  determines  whether  two  circles  intersect.  1  is  returned  if 
they  do. 

*  Circle  1  has  center  cl  and  radius  rl.  Similarly  for  circle  2. 

*/ 

int  ccintersect (uavlnfo* ,vect3, double, vect3, double) ; 

//  changes  a  vector  to  a  unit  vector 
void  makeUnitVector (vect3*) ; 

//  for  a  vector  with  beginning  point  >bi  and  ending  point  ’e*  and  a  point  *p’ 
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//  this  function  returns  -1  for  ’p’  on  the  right  of  the  vector,  1  for  'p'  on 
//  the  left  of  the  vector  and  0  for  ’p’  on  the  vector 
int  getSideOf Line (vect3*  b,  vect3*  e,  vect3*  p) ; 

//  if  the  uav  is  flying  too  far  off  the  path  to  intersect  the  circle 
//  then  this  function  is  called.  It  calculates  the  point  where  the 
//  circles  should  intersect,  then  it  creates  a  line  from  that  point 
//  perpendicular  to  the  current  path  segment  and  checks  to  see  if  the 
//  circle  has  crossed  the  line.  If  it  has  it  returns  tame 
int  of f PathCCintersect (uavlnf o* ,vect3  c,  double  r,  vect3 
sideCircle) ; 

//  this  function  takes  a  line  defined  by  a  starting  and  ending  point 
//  and  moves  it  a  distance  ’dist’  to  the  right  or  the  left  side  of 
//  the  line,  ’b’  and  ’e’  are  changed  to  the  new  offset  line 
void  of f setLine (vect3*  b,  vect3*  e,  double  dist,  int  side); 

//  this  function  figures  out  where  the  center  of  the  turning  circle  should 
//be  given  that  the  path  calls  for  doubling  back  on  itself  -  i.e.  a  U  turn 
//  this  will  preserve  path  length 

void  getUturncenter(vect3  sigmaO,  vect3  sigmai,  double  r,  vect3* 
c) ; 

//  computes  the  ’delta’  function  on  page  45  of  Erik  Anderson’s  thesis. 

//  This  is  the  distance  to  complete  half  of  a  kappa  turn.  This  is  used 

//  to  tell  if  a  path  segment  is  too  short  -  i.e.  if  half  the  distance  of 

//  the  turn  into  the  path  segment  plus  half  of  the  distance  of  the  turn 

//  out  of  the  path  segment  is  longer  than  the  path  segment  length,  then 
//  the  length  of  the  segment  is  too  short, 
double  half KappaDist (double  kappa,  double  beta,  double  r) ; 

#endif 


10.3  Other  files 

Supporting  DLL  files  are  on  the  CD  accompanying  the  report. 
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